Push old dev_2 draft to work on other things.

- **NON-FUNCTIONAL**
- Contains an old draft of separating the stepper driver direct access
to the planner buffer. This is designed to keep the stepper and planner
modules independent and prevent overwriting or other complications. In
this way, feedrate override should be able to be installed as well.
- A number of planner optimizations are installed too.
- Not sure where the bugs are. Either in the new planner optimizations,
new stepper module updates, or in both. Or it just could be that the
Arduino AVR is choking with the new things it has to do.

config.h

@@ -25,7 +25,7 @@
 // IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them.
 
 // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h
-#define DEFAULTS_GENERIC
+#define DEFAULTS_ZEN_TOOLWORKS_7x7
 
 // Serial baud rate
 #define BAUD_RATE 9600
@@ -113,7 +113,7 @@
 // interrupt of the current stepper driver algorithm theoretically up to a frequency of 35-40kHz, but 
 // CPU overhead increases exponentially as this frequency goes up. So there will be little left for 
 // other processes like arcs.  
-#define ISR_TICKS_PER_SECOND 30000L  // Integer (Hz)
+#define ISR_TICKS_PER_SECOND 20000L  // Integer (Hz)
 
 // The temporal resolution of the acceleration management subsystem. Higher number give smoother
 // acceleration but may impact performance. If you run at very high feedrates (>15kHz or so) and 
@@ -121,7 +121,8 @@
 // profiles and how the stepper program actually performs them. The correct value for this parameter
 // is machine dependent, so it's advised to set this only as high as needed. Approximate successful
 // values can widely range from 50 to 200 or more. Cannot be greater than ISR_TICKS_PER_SECOND/2.
-#define ACCELERATION_TICKS_PER_SECOND 120L 
+// NOTE: Ramp count variable type in stepper module may need to be updated if changed.
+#define ACCELERATION_TICKS_PER_SECOND 100L 
 
 // NOTE: Make sure this value is less than 256, when adjusting both dependent parameters.
 #define ISR_TICKS_PER_ACCELERATION_TICK (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)
@@ -134,7 +135,7 @@
 // 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 RANADE_MULTIPLIER 100000000.0
+#define INV_TIME_MULTIPLIER 10000000.0
 
 // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end
 // of the buffer and all stops. This should not be much greater than zero and should only be changed

gcode.c

@@ -39,7 +39,9 @@ parser_state_t gc;
 
 #define FAIL(status) gc.status_code = status;
 
-static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter);
+static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter);
+static void gc_convert_arc_radius_mode(float *target) __attribute__((noinline));
+
 
 static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) 
 {
@@ -48,6 +50,7 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
   gc.plane_axis_2 = axis_2;
 }
 
+
 void gc_init() 
 {
   memset(&gc, 0, sizeof(gc));
@@ -61,20 +64,24 @@ void gc_init()
   } 
 }
 
+
 // Sets g-code parser position in mm. Input in steps. Called by the system abort and hard
 // limit pull-off routines.
-void gc_set_current_position(int32_t x, int32_t y, int32_t z) 
+void gc_sync_position(int32_t x, int32_t y, int32_t z) 
 {
-  gc.position[X_AXIS] = x/settings.steps_per_mm[X_AXIS];
+  uint8_t i;
-  gc.position[Y_AXIS] = y/settings.steps_per_mm[Y_AXIS];
+  for (i=0; i<N_AXIS; i++) {
-  gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS]; 
+    gc.position[i] = sys.position[i]/settings.steps_per_mm[i];
+  }
 }
 
+
 static float to_millimeters(float value) 
 {
   return(gc.inches_mode ? (value * MM_PER_INCH) : value);
 }
 
+         
 // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
 // characters and signed floating point values (no whitespace). Comments and block delete
 // characters have been removed. In this function, all units and positions are converted and 
@@ -99,9 +106,11 @@ uint8_t gc_execute_line(char *line)
   uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53}
   uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal)
   
-  float target[N_AXIS], offset[N_AXIS];
+  float target[N_AXIS];
   clear_vector(target); // XYZ(ABC) axes parameters.
-  clear_vector(offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
+
+  gc.arc_radius = 0;
+  clear_vector(gc.arc_offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
 
   gc.status_code = STATUS_OK;
   
@@ -208,7 +217,7 @@ uint8_t gc_execute_line(char *line)
      NOTE: Grbl unconventionally pre-converts these parameter values based on the block G and M 
      commands. This is set out of the order of execution defined by NIST only for code efficiency/size 
      purposes, but should not affect proper g-code execution. */ 
-  float p = 0, r = 0;
+  float p = 0;
   uint8_t l = 0;
   char_counter = 0;
   while(next_statement(&letter, &value, line, &char_counter)) {
@@ -222,10 +231,10 @@ uint8_t gc_execute_line(char *line)
           gc.feed_rate = to_millimeters(value); // millimeters per minute
         }
         break;
-      case 'I': case 'J': case 'K': offset[letter-'I'] = to_millimeters(value); break;
+      case 'I': case 'J': case 'K': gc.arc_offset[letter-'I'] = to_millimeters(value); break;
       case 'L': l = trunc(value); break;
       case 'P': p = value; break;                    
-      case 'R': r = to_millimeters(value); break;
+      case 'R': gc.arc_radius = to_millimeters(value); break;
       case 'S': 
         if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
         // TBD: Spindle speed not supported due to PWM issues, but may come back once resolved.
@@ -245,7 +254,8 @@ uint8_t gc_execute_line(char *line)
   // If there were any errors parsing this line, we will return right away with the bad news
   if (gc.status_code) { return(gc.status_code); }
   
-  uint8_t i;
+  // Initialize axis index
+  uint8_t idx;
 
   /* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41. */
     
@@ -292,12 +302,12 @@ uint8_t gc_execute_line(char *line)
         float coord_data[N_AXIS];
         if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
         // Update axes defined only in block. Always in machine coordinates. Can change non-active system.
-        for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
+        for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
-          if (bit_istrue(axis_words,bit(i)) ) {
+          if (bit_istrue(axis_words,bit(idx)) ) {
             if (l == 20) {
-              coord_data[i] = gc.position[i]-target[i]; // L20: Update axis current position to target
+              coord_data[idx] = gc.position[idx]-target[idx]; // L20: Update axis current position to target
             } else {
-              coord_data[i] = target[i]; // L2: Update coordinate system axis
+              coord_data[idx] = target[idx]; // L2: Update coordinate system axis
             }
           }
         }
@@ -312,15 +322,15 @@ uint8_t gc_execute_line(char *line)
       // and absolute and incremental modes.
       if (axis_words) {
         // Apply absolute mode coordinate offsets or incremental mode offsets.
-        for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
+        for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
-          if ( bit_istrue(axis_words,bit(i)) ) {
+          if ( bit_istrue(axis_words,bit(idx)) ) {
             if (gc.absolute_mode) {
-              target[i] += gc.coord_system[i] + gc.coord_offset[i];
+              target[idx] += gc.coord_system[idx] + gc.coord_offset[idx];
             } else {
-              target[i] += gc.position[i];
+              target[idx] += gc.position[idx];
             }
           } else {
-            target[i] = gc.position[i];
+            target[idx] = gc.position[idx];
           }
         }
         mc_line(target, -1.0, false);
@@ -349,9 +359,9 @@ uint8_t gc_execute_line(char *line)
       } else {
         // Update axes defined only in block. Offsets current system to defined value. Does not update when
         // active coordinate system is selected, but is still active unless G92.1 disables it. 
-        for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
+        for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
-          if (bit_istrue(axis_words,bit(i)) ) {
+          if (bit_istrue(axis_words,bit(idx)) ) {
-            gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
+            gc.coord_offset[idx] = gc.position[idx]-gc.coord_system[idx]-target[idx];
           }
         }
       }
@@ -383,17 +393,17 @@ uint8_t gc_execute_line(char *line)
     // Convert all target position data to machine coordinates for executing motion. Apply
     // absolute mode coordinate offsets or incremental mode offsets.
     // NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
-    for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used to save flash space.
+    for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used to save flash space.
-      if ( bit_istrue(axis_words,bit(i)) ) {
+      if ( bit_istrue(axis_words,bit(idx)) ) {
         if (!absolute_override) { // Do not update target in absolute override mode
           if (gc.absolute_mode) {
-            target[i] += gc.coord_system[i] + gc.coord_offset[i]; // Absolute mode
+            target[idx] += gc.coord_system[idx] + gc.coord_offset[idx]; // Absolute mode
           } else {
-            target[i] += gc.position[i]; // Incremental mode
+            target[idx] += gc.position[idx]; // Incremental mode
           }
         }
       } else {
-        target[i] = gc.position[i]; // No axis word in block. Keep same axis position.
+        target[idx] = gc.position[idx]; // No axis word in block. Keep same axis position.
       }
     }
   
@@ -417,12 +427,79 @@ uint8_t gc_execute_line(char *line)
         // Check if at least one of the axes of the selected plane has been specified. If in center 
         // format arc mode, also check for at least one of the IJK axes of the selected plane was sent.
         if ( !( bit_false(axis_words,bit(gc.plane_axis_2)) ) || 
-             ( !r && !offset[gc.plane_axis_0] && !offset[gc.plane_axis_1] ) ) { 
+             ( !gc.arc_radius && !gc.arc_offset[gc.plane_axis_0] && !gc.arc_offset[gc.plane_axis_1] ) ) { 
           FAIL(STATUS_INVALID_STATEMENT);
         } else {
-          if (r != 0) { // Arc Radius Mode
+          if (gc.arc_radius != 0) { // Arc Radius Mode
-            /* 
+            // Compute arc radius and offsets
-              We need to calculate the center of the circle that has the designated radius and passes
+            gc_convert_arc_radius_mode(target);
+            if (gc.status_code) { return(gc.status_code); }
+          } else { // Arc Center Format Offset Mode            
+            gc.arc_radius = hypot(gc.arc_offset[gc.plane_axis_0], gc.arc_offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
+          }
+          
+          // Set clockwise/counter-clockwise sign for mc_arc computations
+          uint8_t isclockwise = false;
+          if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
+    
+          // Trace the arc
+          mc_arc(gc.position, target, gc.arc_offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
+            (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
+            gc.arc_radius, isclockwise);
+        }            
+        break;
+    }
+    
+    // Report any errors.
+    if (gc.status_code) { return(gc.status_code); }    
+    
+    // As far as the parser is concerned, the position is now == target. In reality the
+    // motion control system might still be processing the action and the real tool position
+    // in any intermediate location.
+    memcpy(gc.position, target, sizeof(target)); // gc.position[] = target[];
+  }
+  
+  // M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may 
+  // refill and can only be resumed by the cycle start run-time command.
+  if (gc.program_flow) {
+    plan_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
+    sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
+    
+    // If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
+    // re-enable program flow after pause complete, where cycle start will resume the program.
+    if (gc.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
+    else { gc.program_flow = PROGRAM_FLOW_RUNNING; }
+  }    
+  
+  return(gc.status_code);
+}
+
+// Parses the next statement and leaves the counter on the first character following
+// the statement. Returns 1 if there was a statements, 0 if end of string was reached
+// or there was an error (check state.status_code).
+static uint8_t next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) 
+{
+  if (line[*char_counter] == 0) {
+    return(0); // No more statements
+  }
+  
+  *letter = line[*char_counter];
+  if((*letter < 'A') || (*letter > 'Z')) {
+    FAIL(STATUS_EXPECTED_COMMAND_LETTER);
+    return(0);
+  }
+  (*char_counter)++;
+  if (!read_float(line, char_counter, float_ptr)) {
+    FAIL(STATUS_BAD_NUMBER_FORMAT); 
+    return(0);
+  };
+  return(1);
+}
+
+
+static void gc_convert_arc_radius_mode(float *target)  
+{
+/*  We need to calculate the center of the circle that has the designated radius and passes
     through both the current position and the target position. This method calculates the following
     set of equations where [x,y] is the vector from current to target position, d == magnitude of 
     that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
@@ -467,19 +544,17 @@ uint8_t gc_execute_line(char *line)
  
     h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
     i = (x - (y * h_x2_div_d))/2
-              j = (y + (x * h_x2_div_d))/2
+    j = (y + (x * h_x2_div_d))/2        */
-              
-            */
   
   // Calculate the change in position along each selected axis
   float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
   float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
   
-            clear_vector(offset);
+  clear_vector(gc.arc_offset);
   // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
   // than d. If so, the sqrt of a negative number is complex and error out.
-            float h_x2_div_d = 4 * r*r - x*x - y*y;
+  float h_x2_div_d = 4 * gc.arc_radius*gc.arc_radius - x*x - y*y;
-            if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); }
+  if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return; }
   // Finish computing h_x2_div_d.
   h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
   // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
@@ -506,74 +581,13 @@ uint8_t gc_execute_line(char *line)
   // even though it is advised against ever generating such circles in a single line of g-code. By 
   // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
   // travel and thus we get the unadvisably long arcs as prescribed.
-            if (r < 0) { 
+  if (gc.arc_radius < 0) { 
       h_x2_div_d = -h_x2_div_d; 
-                r = -r; // Finished with r. Set to positive for mc_arc
+      gc.arc_radius = -gc.arc_radius; // Finished with r. Set to positive for mc_arc
   }        
   // Complete the operation by calculating the actual center of the arc
-            offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
+  gc.arc_offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
-            offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
+  gc.arc_offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
-
-          } else { // Arc Center Format Offset Mode            
-            r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
-          }
-          
-          // Set clockwise/counter-clockwise sign for mc_arc computations
-          uint8_t isclockwise = false;
-          if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
-    
-          // Trace the arc
-          mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
-            (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
-            r, isclockwise);
-        }            
-        break;
-    }
-    
-    // Report any errors.
-    if (gc.status_code) { return(gc.status_code); }    
-    
-    // As far as the parser is concerned, the position is now == target. In reality the
-    // motion control system might still be processing the action and the real tool position
-    // in any intermediate location.
-    memcpy(gc.position, target, sizeof(target)); // gc.position[] = target[];
-  }
-  
-  // M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may 
-  // refill and can only be resumed by the cycle start run-time command.
-  if (gc.program_flow) {
-    plan_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
-    sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
-    
-    // If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
-    // re-enable program flow after pause complete, where cycle start will resume the program.
-    if (gc.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
-    else { gc.program_flow = PROGRAM_FLOW_RUNNING; }
-  }    
-  
-  return(gc.status_code);
-}
-
-// Parses the next statement and leaves the counter on the first character following
-// the statement. Returns 1 if there was a statements, 0 if end of string was reached
-// or there was an error (check state.status_code).
-static int next_statement(char *letter, float *float_ptr, char *line, uint8_t *char_counter) 
-{
-  if (line[*char_counter] == 0) {
-    return(0); // No more statements
-  }
-  
-  *letter = line[*char_counter];
-  if((*letter < 'A') || (*letter > 'Z')) {
-    FAIL(STATUS_EXPECTED_COMMAND_LETTER);
-    return(0);
-  }
-  (*char_counter)++;
-  if (!read_float(line, char_counter, float_ptr)) {
-    FAIL(STATUS_BAD_NUMBER_FORMAT); 
-    return(0);
-  };
-  return(1);
 }   
 
 /* 

gcode.h

@@ -83,6 +83,10 @@ typedef struct {
                                    // position in mm. Loaded from EEPROM when called.
   float coord_offset[N_AXIS];      // Retains the G92 coordinate offset (work coordinates) relative to
                                    // machine zero in mm. Non-persistent. Cleared upon reset and boot.    
+                                   
+  float arc_radius; 
+  float arc_offset[N_AXIS];
+                                         
 } parser_state_t;
 extern parser_state_t gc;
 
@@ -93,6 +97,6 @@ void gc_init();
 uint8_t gc_execute_line(char *line);
 
 // Set g-code parser position. Input in steps.
-void gc_set_current_position(int32_t x, int32_t y, int32_t z); 
+void gc_sync_position(); 
 
 #endif

limits.c

@@ -96,16 +96,17 @@ static void homing_cycle(uint8_t cycle_mask, int8_t pos_dir, bool invert_pin, fl
   // and speedy homing routine.
   // NOTE: For each axes enabled, the following calculations assume they physically move 
   // an equal distance over each time step until they hit a limit switch, aka dogleg.
-  uint32_t step_event_count, steps[N_AXIS];
+  uint32_t step_event_count = 0;
   uint8_t i, dist = 0;
+  uint32_t steps[N_AXIS];
   clear_vector(steps);
   for (i=0; i<N_AXIS; i++) {
     if (cycle_mask & (1<<i)) { 
       dist++;
       steps[i] = lround(settings.steps_per_mm[i]); 
+      step_event_count = max(step_event_count,steps[i]);
     }
   }
-  step_event_count = max(steps[X_AXIS], max(steps[Y_AXIS], steps[Z_AXIS]));  
   
   // To ensure global acceleration is not exceeded, reduce the governing axes nominal rate
   // by adjusting the actual axes distance traveled per step. This is the same procedure
@@ -244,9 +245,9 @@ void limits_go_home()
 // and the workspace volume is in all negative space.
 void limits_soft_check(float *target)
 {
-  if ( target[X_AXIS] > 0 || target[X_AXIS] < -settings.max_travel[X_AXIS] || 
+  uint8_t idx;
-       target[Y_AXIS] > 0 || target[Y_AXIS] < -settings.max_travel[Y_AXIS] || 
+  for (idx=0; idx<N_AXIS; idx++) { 
-       target[Z_AXIS] > 0 || target[Z_AXIS] < -settings.max_travel[Z_AXIS] ) {
+    if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) {  // NOTE: max_travel is stored as negative
     
       // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within 
       // workspace volume so just come to a controlled stop so position is not lost. When complete
@@ -262,5 +263,8 @@ void limits_soft_check(float *target)
       mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
       sys.execute |= EXEC_CRIT_EVENT; // Indicate soft limit critical event
       protocol_execute_runtime(); // Execute to enter critical event loop and system abort
+      return;
+    
+    }
   }
 }

main.c

@@ -72,7 +72,8 @@ int main(void)
 
       // Sync cleared gcode and planner positions to current system position, which is only
       // cleared upon startup, not a reset/abort. 
-      sys_sync_current_position();
+      plan_sync_position();
+      gc_sync_position();
 
       // Reset system variables.
       sys.abort = false;
@@ -101,12 +102,12 @@ int main(void)
     }
     
     protocol_execute_runtime();
-    protocol_process(); // ... process the serial protocol
     
     // When the serial protocol returns, there are no more characters in the serial read buffer to
     // be processed and executed. This indicates that individual commands are being issued or 
     // streaming is finished. In either case, auto-cycle start, if enabled, any queued moves.
     mc_auto_cycle_start();
+    protocol_process(); // ... process the serial protocol
     
   }
   return 0;   /* never reached */

motion_control.c

@@ -72,7 +72,7 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)
     else { break; }
   } while (1);
 
-  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
+  plan_buffer_line(target, feed_rate, invert_feed_rate);
 
   // If idle, indicate to the system there is now a planned block in the buffer ready to cycle 
   // start. Otherwise ignore and continue on.
@@ -148,8 +148,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
        This is important when there are successive arc motions. 
     */
     // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec
-    float cos_T = 2 - theta_per_segment*theta_per_segment;
+    float cos_T = 2.0 - theta_per_segment*theta_per_segment;
-    float sin_T = theta_per_segment*0.16666667*(cos_T + 4);
+    float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
     cos_T *= 0.5;
 
     float arc_target[N_AXIS];
@@ -229,37 +229,34 @@ void mc_go_home()
   // At the same time, set up pull-off maneuver from axes limit switches that have been homed.
   // This provides some initial clearance off the switches and should also help prevent them 
   // from falsely tripping when hard limits are enabled.
-  // TODO: Need to improve dir_mask[] to be more axes independent.
   float pulloff_target[N_AXIS];
   clear_vector_float(pulloff_target); // Zero pulloff target.
   clear_vector_long(sys.position); // Zero current position for now.
-  uint8_t dir_mask[N_AXIS];
+  uint8_t idx;
-  dir_mask[X_AXIS] = (1<<X_DIRECTION_BIT);
+  for (idx=0; idx<N_AXIS; idx++) {
-  dir_mask[Y_AXIS] = (1<<Y_DIRECTION_BIT);
-  dir_mask[Z_AXIS] = (1<<Z_DIRECTION_BIT);
-  uint8_t i;
-  for (i=0; i<N_AXIS; i++) {
     // Set up pull off targets and machine positions for limit switches homed in the negative
     // direction, rather than the traditional positive. Leave non-homed positions as zero and
     // do not move them.
-    if (HOMING_LOCATE_CYCLE & bit(i)) {
+    // NOTE: settings.max_travel[] is stored as a negative value.
-      if (settings.homing_dir_mask & dir_mask[i]) {
+    if (HOMING_LOCATE_CYCLE & bit(idx)) {
-        pulloff_target[i] = settings.homing_pulloff-settings.max_travel[i];
+      if ( settings.homing_dir_mask & get_direction_mask(idx) ) {
-        sys.position[i] = -lround(settings.max_travel[i]*settings.steps_per_mm[i]);
+        pulloff_target[idx] = settings.homing_pulloff+settings.max_travel[idx];
+        sys.position[idx] = lround(settings.max_travel[idx]*settings.steps_per_mm[idx]);
       } else {
-        pulloff_target[i] = -settings.homing_pulloff;
+        pulloff_target[idx] = -settings.homing_pulloff;
       }
     }
   }
-  sys_sync_current_position();
+  plan_sync_position(); // Sync planner position to home for pull-off move.
+
   sys.state = STATE_IDLE; // Set system state to IDLE to complete motion and indicate homed.
 
   mc_line(pulloff_target, settings.homing_seek_rate, false);
   st_cycle_start(); // Move it. Nothing should be in the buffer except this motion. 
   plan_synchronize(); // Make sure the motion completes.
   
-  // The gcode parser position circumvented by the pull-off maneuver, so sync position vectors.
+  // The gcode parser position circumvented by the pull-off maneuver, so sync position now.
-  sys_sync_current_position();
+  gc_sync_position();
 
   // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
   if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { LIMIT_PCMSK |= LIMIT_MASK; }

nuts_bolts.c

@@ -140,9 +140,16 @@ void delay_us(uint32_t us)
   }
 }
 
-// Syncs all internal position vectors to the current system position.
+
-void sys_sync_current_position()
+// Returns direction mask according to Grbl internal axis indexing.
+uint8_t get_direction_mask(uint8_t axis_idx)
 {
-  plan_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
+  uint8_t axis_mask = 0;
-  gc_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS]);
+  switch( axis_idx ) {
+    case X_AXIS: axis_mask = (1<<X_DIRECTION_BIT); break;
+    case Y_AXIS: axis_mask = (1<<Y_DIRECTION_BIT); break;
+    case Z_AXIS: axis_mask = (1<<Z_DIRECTION_BIT); break;
+  }
+  return(axis_mask);
 }
+

nuts_bolts.h

@@ -105,7 +105,6 @@ void delay_ms(uint16_t ms);
 // Delays variable-defined microseconds. Compiler compatibility fix for _delay_us().
 void delay_us(uint32_t us);
 
-// Syncs Grbl's gcode and planner position variables with the system position.
+uint8_t get_direction_mask(uint8_t i);
-void sys_sync_current_position();
 
 #endif

planner.c

@@ -2,8 +2,8 @@
   planner.c - buffers movement commands and manages the acceleration profile plan
   Part of Grbl
 
-  Copyright (c) 2009-2011 Simen Svale Skogsrud
   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
@@ -34,11 +34,11 @@
 #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 block_t block_buffer[BLOCK_BUFFER_SIZE];  // A ring buffer for motion instructions
+static plan_block_t block_buffer[BLOCK_BUFFER_SIZE];  // A ring buffer for motion instructions
-static volatile uint8_t block_buffer_head;       // Index of the next block to be pushed
 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 *block_t block_buffer_planned;
+static uint8_t block_buffer_planned;             // Index of the optimally planned block
 
 // Define planner variables
 typedef struct {
@@ -47,7 +47,6 @@ typedef struct {
                                      // 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
-  float last_x, last_y, last_z;      // Target position of previous path line segment
 } planner_t;
 static planner_t pl;
 
@@ -71,64 +70,6 @@ static uint8_t prev_block_index(uint8_t block_index)
 }
                             
 
-/*       STEPPER VELOCITY PROFILE DEFINITION 
-                                                  less than nominal rate->  + 
-                    +--------+ <- nominal_rate                             /|\                                         
-                   /          \                                           / | \                      
-  initial_rate -> +            \                                         /  |  + <- next->initial_rate         
-                  |             + <- next->initial_rate                 /   |  |                       
-                  +-------------+                      initial_rate -> +----+--+                   
-                   time -->  ^  ^                                           ^  ^                       
-                             |  |                                           |  |                       
-                     decelerate distance                           decelerate distance  
-                                                        
-  Calculates trapezoid parameters for stepper algorithm. 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. Some other special cases exist, 
-  such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that
-  has no deceleration period when the next block resumes acceleration. 
-  
-  The following function determines the type of velocity profile and stores the minimum required
-  information for the stepper algorithm to execute the calculated profiles. In this case, only
-  the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
-  already handles acceleration and cruising and just needs to know when to start decelerating.
-*/
-static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) 
-{  
-  // Compute new initial rate for stepper algorithm
-  block->initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
-          
-  // TODO: Compute new nominal rate if a feedrate override occurs.
-  // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
-    
-  // Compute efficiency variable for following calculations. Removes a float divide and multiply.
-  // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds.
-  float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); 
-  
-  // First determine intersection distance (in steps) from the exit point for a triangular profile.
-  // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) )
-  int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) );  
-  
-  // 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) {
-    block->decelerate_after = 0; 
-  } else {
-    // 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: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) )
-    block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr));
-
-    // The lesser of the two triangle and trapezoid distances always defines the velocity profile.
-    if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; }
-    
-    // Finally, check if this is a pure deceleration block.
-    if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; }
-  }
-}     
-                                        
-
 /*                            PLANNER SPEED DEFINITION                                              
                                      +--------+   <- current->nominal_speed
                                     /          \                                
@@ -185,178 +126,130 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr,
 */
 static void planner_recalculate() 
 {     
+  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
+  uint8_t block_index = block_buffer_head;
+  plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer
   
-//   float entry_speed_sqr;
+  // Ping the stepper algorithm to check if we can alter the parameters of the currently executing
-//   uint8_t block_index = block_buffer_head;
+  // block. If not, skip it and work on the next block. 
-//   block_t *previous = NULL;
+  // TODO: Need to look into if there are conditions where this fails.
-//   block_t *current = NULL;
+  uint8_t block_buffer_safe = next_block_index( block_buffer_tail );
-//   block_t *next;
-//   while (block_index != block_buffer_tail) {
-//     block_index = prev_block_index( block_index );
-//     next = current;
-//     current = previous;
-//     previous = &block_buffer[block_index];
-//     
-//     if (next && current) {
-//       if (next != block_buffer_planned) {
-//         if (previous == block_buffer_tail) { block_buffer_planned = next; }
-//         else {
-//         
-//           if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
-//             current->recalculate_flag = true; // Almost always changes. So force recalculate.       
-//             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;
-//             }
-//           } else {  
-//             block_buffer_planned = current;
-//           }
-//         }
-//       } else { 
-//         break;
-//       }
-//     }
-//   } 
-// 
-//   block_index = block_buffer_planned;
-//   next = &block_buffer[block_index];
-//   current = prev_block_index(block_index);
-//   while (block_index != block_buffer_head) {
-// 
-//       // If the current block is an acceleration block, but it is not long enough to complete the
-//       // full speed change within the block, we need to adjust the exit speed accordingly. Entry
-//       // speeds have already been reset, maximized, and reverse planned by reverse planner.
-//       if (current->entry_speed_sqr < next->entry_speed_sqr) {
-//         // Compute block exit speed based on the current block speed and distance
-//         // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance
-//         entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
-//         
-//         // If it's less than the stored value, update the exit speed and set recalculate flag.
-//         if (entry_speed_sqr < next->entry_speed_sqr) {
-//           next->entry_speed_sqr = entry_speed_sqr;
-//           next->recalculate_flag = true;
-//         }
-//       }
-// 
-//       // Recalculate if current block entry or exit junction speed has changed.
-//       if (current->recalculate_flag || next->recalculate_flag) {
-//         // NOTE: Entry and exit factors always > 0 by all previous logic operations.     
-//         calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);      
-//         current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
-//       }
-//       
-//     current = next;
-//     next = &block_buffer[block_index];
-//     block_index = next_block_index( block_index );
-//   }
-//   
-//   // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
-//   calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
-//   next->recalculate_flag = false;
   
-  // TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but 
+  // TODO: Need to recompute buffer tail millimeters based on how much is completed.
-  // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to
-  // find out where in the planner buffer is the a safe place to begin re-planning from.
   
-//   if (block_buffer_head != block_buffer_tail) {   
+  if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on
+
+    block_buffer_planned = block_buffer_safe;
+//     calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
+
+  } else {
+  
+    // TODO: need to account for the two block condition better. If the currently executing block
+    // is not safe, do we wait until its done? Can we treat the buffer head differently?
+  
+    // Calculate trapezoid for the last/newest block.
+    current->entry_speed_sqr = min( current->max_entry_speed_sqr,
+       MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters);
+//     calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
+
+      
+    // Reverse Pass: Back plan the deceleration curve 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.
     float entry_speed_sqr;
-
+    plan_block_t *next;
-  // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the
+    block_index = prev_block_index(block_index);
-  // tail(first) block to prevent over-writing of the initial entry speed. 
+    while (block_index != block_buffer_planned) {
-  uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty.
-  block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block
-  block_t *next;
-  if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); }
-  while (block_index != block_buffer_tail) {
       next = current;
       current = &block_buffer[block_index];
       
-    // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter
+      // Exit loop and update planned pointer when the tail/safe block is reached.
-    // the planner nominal speeds at any time. This calc could be done in the override handler, but 
+      if (block_index == block_buffer_safe) {
-    // this could require an additional variable to be stored to differentiate the programmed nominal
+        block_buffer_planned = block_buffer_safe;
-    // speeds, max junction speed, and override speeds/scalar.
+        break;
+      }
       
-    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
+      // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to
-    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and 
+      // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan.
-    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
       if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
-
-      current->entry_speed_sqr = current->max_entry_speed_sqr;
-      current->recalculate_flag = true; // Almost always changes. So force recalculate.       
-
-      if (next->entry_speed_sqr < current->max_entry_speed_sqr) {
-        // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance      
         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;
         }
       }
-    }    
+      block_index = prev_block_index(block_index);
-    block_index = prev_block_index( block_index );
     }
 
-  // Perform forward planner pass. Begins junction speed adjustments after tail(first) block.
+    // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
-  // Also recalculate trapezoids, block by block, as the forward pass completes the plan.
+    // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
-  block_index = next_block_index(block_buffer_tail);
+    block_index = block_buffer_planned; // Begin at buffer planned pointer
-  next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current
+    next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop
-  while (block_index != block_buffer_head) {
+    while (block_index != next_buffer_head) {
       current = next;
       next = &block_buffer[block_index];
       
-      // If the current block is an acceleration block, but it is not long enough to complete the
+      // Any acceleration detected in the forward pass automatically moves the optimal planned
-      // full speed change within the block, we need to adjust the exit speed accordingly. Entry
+      // pointer forward, since everything before this is all optimal. In other words, nothing
-      // speeds have already been reset, maximized, and reverse planned by reverse planner.
+      // can improve the plan from the buffer tail to the planned pointer by logic.
       if (current->entry_speed_sqr < next->entry_speed_sqr) {
-        // Compute block exit speed based on the current block speed and distance
+        block_buffer_planned = block_index;
-        // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance
         entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
-        
-        // If it's less than the stored value, update the exit speed and set recalculate flag.
         if (entry_speed_sqr < next->entry_speed_sqr) {
-          next->entry_speed_sqr = entry_speed_sqr;
+          next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this.
-          next->recalculate_flag = true;
         }
       }
       
-      // Recalculate if current block entry or exit junction speed has changed.
+      // Any block set at its maximum entry speed also creates an optimal plan up to this
-      if (current->recalculate_flag || next->recalculate_flag) {
+      // point in the buffer. The optimally planned pointer is updated.
-        // NOTE: Entry and exit factors always > 0 by all previous logic operations.     
+      if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
-        calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);      
+        block_buffer_planned = block_index;
-        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
       }
       
+      // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned
+      // pointer to the end of the buffer, except the last block.
+//       calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
       block_index = next_block_index( block_index );
     }
     
-  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
+  }  
-  calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
+  
-  next->recalculate_flag = false;
-//   }
 }
 
+
 void plan_init() 
 {
+  block_buffer_head = 0;
   block_buffer_tail = block_buffer_head;
   next_buffer_head = next_block_index(block_buffer_head);
-//   block_buffer_planned = block_buffer_head;
+  block_buffer_planned = block_buffer_head;
   memset(&pl, 0, sizeof(pl)); // Clear planner struct
 }
 
-inline void plan_discard_current_block() 
+
+void plan_discard_current_block() 
 {
   if (block_buffer_head != block_buffer_tail) {
     block_buffer_tail = next_block_index( block_buffer_tail );
   }
 }
 
-inline block_t *plan_get_current_block() 
+
+plan_block_t *plan_get_current_block() 
 {
   if (block_buffer_head == block_buffer_tail) { 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()
 {
@@ -364,6 +257,7 @@ uint8_t plan_check_full_buffer()
   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()
@@ -374,42 +268,53 @@ void plan_synchronize()
   }    
 }
 
-// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
+
-// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
+// 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.
-// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate
+// In other words, the buffer head is never equal to the buffer tail.  Also the feed rate input value
-// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate
+// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if
-// value is negative (and invert_feed_rate always false).
+// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and
-void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) 
+// invert_feed_rate always false).
+void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) 
 {
-  // Prepare to set up new block
+  // Prepare and initialize new block
-  block_t *block = &block_buffer[block_buffer_head];
+  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
 
-  // Calculate target position in absolute steps
+  // Compute and store initial move distance data.
-  int32_t target[N_AXIS];
+  int32_t target_steps[N_AXIS];
-  target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
+  float unit_vec[N_AXIS], delta_mm;
-  target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
+  uint8_t idx;
-  target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);     
+  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
+    // Number of steps for each axis and determine max step events
-  block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
+    block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
-  block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
+    block->step_event_count = max(block->step_event_count, block->steps[idx]);
-  block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]);
+    
-  block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
+    // 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
-  if (block->step_event_count == 0) { return; };
+  if (block->step_event_count == 0) { return; } 
-  
-  // Compute path vector in terms of absolute step target and current positions
-  float delta_mm[N_AXIS];
-  delta_mm[X_AXIS] = x-pl.last_x;
-  delta_mm[Y_AXIS] = y-pl.last_y; 
-  delta_mm[Z_AXIS] = z-pl.last_z;
-  block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + 
-                            delta_mm[Z_AXIS]*delta_mm[Z_AXIS]);
   
   // 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.
@@ -422,117 +327,151 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
   // and axes properties as well.
   // 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.
-  uint8_t i;
+  float inverse_unit_vec_value;
-  float unit_vec[N_AXIS], inverse_unit_vec_value;
   float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple float divides	
-  block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop
+  float junction_cos_theta = 0;
-  for (i=0; i<N_AXIS; i++) { 
+  for (idx=0; idx<N_AXIS; idx++) {
-    if (delta_mm[i] == 0) { 
+    if (unit_vec[idx] != 0) {  // Avoid divide by zero.
-      unit_vec[i] = 0;  // Store zero value. And avoid divide by zero.
+      unit_vec[idx] *= inverse_millimeters;  // Complete unit vector calculation
-    } else {
+      inverse_unit_vec_value = abs(1.0/unit_vec[idx]); // Inverse to remove multiple float divides.
-      // Compute unit vector and its absolute inverse value
+
-      unit_vec[i] = delta_mm[i]*inverse_millimeters;
+      // Check and limit feed rate against max individual axis velocities and accelerations
-      inverse_unit_vec_value = abs(1.0/unit_vec[i]);
+      feed_rate = min(feed_rate,settings.max_velocity[idx]*inverse_unit_vec_value);
-      // Check and limit feed rate against max axis velocities and scale accelerations to maximums
+      block->acceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value);
-      feed_rate = min(feed_rate,settings.max_velocity[i]*inverse_unit_vec_value);
+
-      block->acceleration = min(block->acceleration,settings.acceleration[i]*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];
     }
   }
 
-  // Compute nominal speed and rates
+  /* Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
-  block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0
+     Let a circle be tangent to both previous and current path line segments, where the junction 
-  block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+     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 
-  // Compute the acceleration and distance traveled per step event for the stepper algorithm.
+     path of centripetal acceleration. Solve for max velocity based on max acceleration about the
-  block->rate_delta = ceil(block->acceleration*
+     radius of the circle, defined indirectly by junction deviation. This may be also viewed as 
-    ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
+     path width or max_jerk in the previous grbl version. This approach does not actually deviate 
-  block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
+     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.
-  // Compute direction bits. Bit enabled always means direction is negative.
+     NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path 
-  block->direction_bits = 0;
+     mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
-  if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
+     stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
-  if (unit_vec[Y_AXIS] < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
+     is exactly the same. Instead of motioning all the way to junction point, the machine will
-  if (unit_vec[Z_AXIS] < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
+     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. 
-  // 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 
+  // TODO: Acceleration need to be limited by the minimum of the two junctions. 
-  // deviation is defined as the distance from the junction to the closest edge of the circle, 
+  // TODO: Need to setup a method to handle zero junction speeds when starting from rest.
-  // colinear with the circle center. The circular segment joining the two paths represents the 
+  if (block_buffer_head == block_buffer_tail) {
-  // 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.
-
-  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
     block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
-  if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 0.0)) {
+  } else {
-    // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
+    // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
-    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
+    float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
-    float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
+    block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2);
-                      - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
-                      - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
-                        
-    // Skip and use default max junction speed for 0 degree acute junction.
-    if (cos_theta < 0.95) {
-      block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr);
-      // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
-      if (cos_theta > -0.95) {
-        // Compute maximum junction velocity based on maximum acceleration and junction deviation
-        float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
-        block->max_entry_speed_sqr = min(block->max_entry_speed_sqr,
-           block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2));
-      }
-    }
   }
 
-  // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED.
+  // Store block nominal speed and rate
-  // TODO: This could be moved to the planner recalculate function.
+  block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0
-  block->entry_speed_sqr = min( block->max_entry_speed_sqr,
+//   block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
-      MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters);
+// 
-
+//   // Compute and store acceleration and distance traveled per step event.
-  // Set new block to be recalculated for conversion to stepper data.
+//   block->rate_delta = ceil(block->acceleration*
-  block->recalculate_flag = true;
+//     ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
+//   block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
 
   // 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, sizeof(target)); // pl.position[] = target[]
+  memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]
-  pl.last_x = x;
-  pl.last_y = y;
-  pl.last_z = z;
-
-  // Update buffer head and next buffer head indices
-  block_buffer_head = next_buffer_head;  
-  next_buffer_head = next_block_index(block_buffer_head);
 
   planner_recalculate(); 
+
+  // Update buffer head and next buffer head indices.
+  // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan
+  // calculations to help prevent overwriting scenarios with adding a new block to a low buffer.
+  block_buffer_head = next_buffer_head;  
+  next_buffer_head = next_block_index(block_buffer_head);
 }
 
+
 // Reset the planner position vectors. Called by the system abort/initialization routine.
-void plan_set_current_position(int32_t x, int32_t y, int32_t z)
+void plan_sync_position()
 {
-  pl.position[X_AXIS] = x;
+  uint8_t idx;
-  pl.position[Y_AXIS] = y;
+  for (idx=0; idx<N_AXIS; idx++) {
-  pl.position[Z_AXIS] = z;
+    pl.position[idx] = sys.position[idx];
-  pl.last_x = x/settings.steps_per_mm[X_AXIS];
+  }
-  pl.last_y = y/settings.steps_per_mm[Y_AXIS];
-  pl.last_z = z/settings.steps_per_mm[Z_AXIS];
 }
 
+
+/*       STEPPER VELOCITY PROFILE DEFINITION 
+                                                  less than nominal rate->  + 
+                    +--------+ <- nominal_rate                             /|\                                         
+                   /          \                                           / | \                      
+  initial_rate -> +            \                                         /  |  + <- next->initial_rate         
+                  |             + <- next->initial_rate                 /   |  |                       
+                  +-------------+                      initial_rate -> +----+--+                   
+                   time -->  ^  ^                                           ^  ^                       
+                             |  |                                           |  |                       
+                     decelerate distance                           decelerate distance  
+                                                        
+  Calculates trapezoid parameters for stepper algorithm. 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. Some other special cases exist, 
+  such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that
+  has no deceleration period when the next block resumes acceleration. 
+  
+  The following function determines the type of velocity profile and stores the minimum required
+  information for the stepper algorithm to execute the calculated profiles. In this case, only
+  the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
+  already handles acceleration and cruising and just needs to know when to start decelerating.
+*/
+int32_t calculate_trapezoid_for_block(uint8_t block_index) 
+{  
+  plan_block_t *current_block = &block_buffer[block_index];
+  
+  // Determine current block exit speed
+  float exit_speed_sqr;
+  uint8_t next_index = next_block_index(block_index);
+  plan_block_t *next_block = plan_get_block_by_index(next_index);
+  if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed.
+  else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block
+  
+  // First determine intersection distance (in steps) from the exit point for a triangular profile.
+  // Computes: steps_intersect = steps/mm * ( 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: steps_decelerate = steps/mm * ( (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) { decelerate_distance = current_block->millimeters; }
+    
+    return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters));
+  }
+  return(0);
+}        
+
+
 // 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) 
 {
-  block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block
+  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
@@ -540,9 +479,9 @@ void plan_cycle_reinitialize(int32_t step_events_remaining)
   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 flags.
+  // 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->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
-  block->recalculate_flag = true;
+  block_buffer_planned = block_buffer_tail;
   planner_recalculate();  
 }

planner.h

@@ -2,8 +2,8 @@
   planner.h - buffers movement commands and manages the acceleration profile plan
   Part of Grbl
 
-  Copyright (c) 2009-2011 Simen Svale Skogsrud
   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
@@ -21,10 +21,11 @@
 
 #ifndef planner_h
 #define planner_h
+#include "nuts_bolts.h"
 
 // The number of linear motions that can be in the plan at any give time
 #ifndef BLOCK_BUFFER_SIZE
-  #define BLOCK_BUFFER_SIZE 17
+  #define BLOCK_BUFFER_SIZE 18
 #endif
 
 // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
@@ -33,42 +34,42 @@ typedef struct {
 
   // Fields used by the bresenham algorithm for tracing the line
   uint8_t direction_bits;            // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
-  uint32_t steps_x, steps_y, steps_z; // Step count along each axis
+  int32_t steps[N_AXIS];             // Step count along each axis
   int32_t step_event_count;          // The number of step events required to complete this block
 
   // Fields used by the motion planner to manage acceleration
-  float nominal_speed_sqr;           // The nominal speed for this block in mm/min  
+  float nominal_speed_sqr;           // Axis-limit adjusted nominal speed for this block in (mm/min)^2
-  float entry_speed_sqr;             // Entry speed at previous-current block junction in mm/min
+  float entry_speed_sqr;             // Entry speed at previous-current block junction in (mm/min)^2
-  float max_entry_speed_sqr;         // Maximum allowable junction entry speed in mm/min
+  float max_entry_speed_sqr;         // Maximum allowable junction entry speed in (mm/min)^2
-  float millimeters;                 // The total travel of this block in mm
+  float acceleration;                // Axes-limit adjusted line acceleration in mm/min^2
-  float acceleration;
+  float millimeters;                 // The total travel for this block to be executed in mm
-  uint8_t recalculate_flag;          // Planner flag to recalculate trapezoids on entry junction
   
   // Settings for the trapezoid generator
-  uint32_t initial_rate;              // The step rate at start of block  
+//   int32_t decelerate_after;  // The index of the step event on which to start decelerating
-  int32_t rate_delta;                 // The steps/minute to add or subtract when changing speed (must be positive)
+
-  uint32_t decelerate_after;          // The index of the step event on which to start decelerating
+} plan_block_t;
-  uint32_t nominal_rate;              // The nominal step rate for this block in step_events/minute
-  uint32_t d_next;                    // Scaled distance to next step
-} block_t;
       
 // Initialize the motion plan subsystem
 void plan_init();
 
-// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
+// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position 
-// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
+// 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.
-void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate);
+void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate);
 
 // Called when the current block is no longer needed. Discards the block and makes the memory
 // availible for new blocks.
 void plan_discard_current_block();
 
 // Gets the current block. Returns NULL if buffer empty
-block_t *plan_get_current_block();
+plan_block_t *plan_get_current_block();
+
+plan_block_t *plan_get_block_by_index(uint8_t block_index);
+
+int32_t calculate_trapezoid_for_block(uint8_t block_index);
 
 // Reset the planner position vector (in steps)
-void plan_set_current_position(int32_t x, int32_t y, int32_t z);
+void plan_sync_position();
 
 // Reinitialize plan with a partially completed block
 void plan_cycle_reinitialize(int32_t step_events_remaining);

planner_old.c

@@ -0,0 +1,476 @@
+/*
+  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 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
+  float last_target[N_AXIS];         // Target position of previous path line segment
+} planner_t;
+static planner_t pl;
+
+
+// Returns the index of the next block in the ring buffer
+// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
+static uint8_t 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 prev_block_index(uint8_t block_index) 
+{
+  if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
+  block_index--;
+  return(block_index);
+}
+
+
+/*       STEPPER VELOCITY PROFILE DEFINITION 
+                                                  less than nominal rate->  + 
+                    +--------+ <- nominal_rate                             /|\                                         
+                   /          \                                           / | \                      
+  initial_rate -> +            \                                         /  |  + <- next->initial_rate         
+                  |             + <- next->initial_rate                 /   |  |                       
+                  +-------------+                      initial_rate -> +----+--+                   
+                   time -->  ^  ^                                           ^  ^                       
+                             |  |                                           |  |                       
+                     decelerate distance                           decelerate distance  
+                                                        
+  Calculates trapezoid parameters for stepper algorithm. 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. Some other special cases exist, 
+  such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that
+  has no deceleration period when the next block resumes acceleration. 
+  
+  The following function determines the type of velocity profile and stores the minimum required
+  information for the stepper algorithm to execute the calculated profiles. In this case, only
+  the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
+  already handles acceleration and cruising and just needs to know when to start decelerating.
+*/
+static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, float exit_speed_sqr) 
+{  
+  // Compute new initial rate for stepper algorithm
+  block->initial_rate = ceil(sqrt(entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+          
+  // TODO: Compute new nominal rate if a feedrate override occurs.
+  // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+    
+  // Compute efficiency variable for following calculations. Removes a float divide and multiply.
+  // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds.
+  float steps_per_mm_div_2_acc = block->step_event_count/(2*block->acceleration*block->millimeters); 
+  
+  // First determine intersection distance (in steps) from the exit point for a triangular profile.
+  // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) )
+  int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed_sqr-exit_speed_sqr)) );  
+  
+  // 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) {
+    block->decelerate_after = 0; 
+  } else {
+    // 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: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) )
+    block->decelerate_after = ceil(steps_per_mm_div_2_acc * (block->nominal_speed_sqr - exit_speed_sqr));
+
+    // The lesser of the two triangle and trapezoid distances always defines the velocity profile.
+    if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; }
+    
+    // Finally, check if this is a pure deceleration block.
+    if (block->decelerate_after > block->step_event_count) { block->decelerate_after = block->step_event_count; }
+  }
+}     
+                                        
+
+/*                            PLANNER SPEED DEFINITION                                              
+                                     +--------+   <- current->nominal_speed
+                                    /          \                                
+         current->entry_speed ->   +            \                               
+                                   |             + <- next->entry_speed
+                                   +-------------+                              
+                                       time -->                      
+                                                  
+  Recalculates the motion plan according to the following algorithm:
+  
+    1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) 
+       so that:
+      a. The junction speed is equal to or less than the maximum junction speed limit
+      b. No speed reduction within one block requires faster deceleration than the acceleration limits.
+      c. The last (or newest appended) block is planned from a complete stop.
+    2. Go over every block in chronological (forward) order and dial down junction speed values if 
+      a. The speed increase within one block would require faster acceleration than the acceleration limits.
+  
+  When these stages are complete, all blocks have a junction entry speed that will allow all speed changes
+  to be performed using the overall limiting acceleration value, and where no junction speed is greater
+  than the max limit. In other words, it just computed the fastest possible velocity profile through all 
+  buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully
+  executed. Finally it will:
+  
+    3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a
+       a planner-modified junction speed with be updated, the others are assumed ok as is.
+  
+  All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only
+  when planned values are converted to stepper rate parameters(3), these are integers. If another motion block
+  is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes.
+  
+  Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's
+  constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and
+  nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile
+  through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting
+  conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile.
+  
+  Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line
+  segments, like arcs, 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 stop at the end of the
+  buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be 
+  able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance.
+  The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED.
+  Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost 
+  steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the 
+  bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory 
+  is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering 
+  up to a hundred or more.
+
+  NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient
+  as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments
+  can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading
+  to weird hiccup-like jerky motions.
+*/
+static void planner_recalculate() 
+{     
+  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
+  uint8_t block_index = block_buffer_head;
+  block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer
+  
+  // Determine safe point for which to plan to.
+  uint8_t block_buffer_safe = next_block_index( block_buffer_tail );
+  
+  if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on
+
+    block_buffer_planned = block_buffer_safe;
+    calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
+
+  } else {
+  
+    // TODO: need to account for the two block condition better. If the currently executing block
+    // is not safe, do we wait until its done? Can we treat the buffer head differently?
+  
+    // Calculate trapezoid for the last/newest block.
+    current->entry_speed_sqr = min( current->max_entry_speed_sqr,
+       MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters);
+    calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
+
+      
+    // Reverse Pass: Back plan the deceleration curve 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.
+    float entry_speed_sqr;
+    block_t *next;
+    block_index = prev_block_index(block_index);
+    while (block_index != block_buffer_planned) {
+      next = current;
+      current = &block_buffer[block_index];
+      
+      // Exit loop and update planned pointer when the tail/safe block is reached.
+      if (block_index == block_buffer_safe) {
+        block_buffer_planned = block_buffer_safe;
+        break;
+      }
+      
+      // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to
+      // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan.
+      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;
+        }
+      }
+      block_index = prev_block_index(block_index);
+    }
+
+    // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
+    // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
+    block_index = block_buffer_planned; // Begin at buffer planned pointer
+    next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop
+    while (block_index != next_buffer_head) {
+      current = next;
+      next = &block_buffer[block_index];
+      
+      // Any acceleration detected in the forward pass automatically moves the optimal planned
+      // pointer forward, since everything before this is all optimal. In other words, nothing
+      // can improve the plan from the buffer tail to the planned pointer by logic.
+      if (current->entry_speed_sqr < next->entry_speed_sqr) {
+        block_buffer_planned = block_index;
+        entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
+        if (entry_speed_sqr < next->entry_speed_sqr) {
+          next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this.
+        }
+      }
+      
+      // Any block set at its maximum entry speed also creates an optimal plan up to this
+      // point in the buffer. The optimally planned pointer is updated.
+      if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
+        block_buffer_planned = block_index;
+      }
+      
+      // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned
+      // pointer to the end of the buffer, except the last block.
+      calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
+      block_index = next_block_index( block_index );
+    }
+    
+  }  
+  
+}
+
+
+void plan_init() 
+{
+  block_buffer_tail = block_buffer_head;
+  next_buffer_head = next_block_index(block_buffer_head);
+  block_buffer_planned = block_buffer_head;
+  memset(&pl, 0, sizeof(pl)); // Clear planner struct
+}
+
+
+inline void plan_discard_current_block() 
+{
+  if (block_buffer_head != block_buffer_tail) {
+    block_buffer_tail = next_block_index( block_buffer_tail );
+  }
+}
+
+
+inline block_t *plan_get_current_block() 
+{
+  if (block_buffer_head == block_buffer_tail) { return(NULL); }
+  return(&block_buffer[block_buffer_tail]);
+}
+
+
+// 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
+  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.
+  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
+    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.
+    delta_mm = target[idx] - pl.last_target[idx];
+    unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later.
+    
+    // Incrementally compute total move distance by Euclidean norm
+    block->millimeters += delta_mm*delta_mm;
+    
+    // Set direction bits. Bit enabled always means direction is negative.
+    if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); }
+  }
+  block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation 
+  
+  // Bail if this is a zero-length block
+  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 limited
+  // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so
+  // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction
+  // and axes properties as well.
+  // 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];
+    }
+  }
+
+  /* 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. 
+  */
+  // TODO: Acceleration need to be limited by the minimum of the two junctions. 
+  // TODO: Need to setup a method to handle zero junction speeds when starting from rest.
+  if (block_buffer_head == block_buffer_tail) {
+    block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
+  } else {
+    // 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.
+    block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2);
+  }
+
+  // Store block nominal speed and rate
+  block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0
+  block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+
+  // Compute and store acceleration and distance traveled per step event.
+  block->rate_delta = ceil(block->acceleration*
+    ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
+  block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
+
+  // 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[]
+  memcpy(pl.last_target, target, sizeof(target)); // pl.last_target[] = target[]
+
+  planner_recalculate(); 
+
+  // Update buffer head and next buffer head indices.
+  // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan
+  // calculations to help prevent overwriting scenarios with adding a new block to a low buffer.
+  block_buffer_head = next_buffer_head;  
+  next_buffer_head = next_block_index(block_buffer_head);
+}
+
+
+// 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];
+    pl.last_target[idx] = sys.position[idx]/settings.steps_per_mm[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) 
+{
+  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 = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
+  block_buffer_planned = block_buffer_tail;
+  planner_recalculate();  
+}

planner_old.h

@@ -0,0 +1,83 @@
+/*
+  planner.h - 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
+
+  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/>.
+*/
+
+#ifndef planner_h
+#define planner_h
+#include "nuts_bolts.h"
+
+// The number of linear motions that can be in the plan at any give time
+#ifndef BLOCK_BUFFER_SIZE
+  #define BLOCK_BUFFER_SIZE 17
+#endif
+
+// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
+// the source g-code and may never actually be reached if acceleration management is active.
+typedef struct {
+
+  // Fields used by the bresenham algorithm for tracing the line
+  uint8_t  direction_bits;            // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
+  uint32_t steps[N_AXIS];             // Step count along each axis
+  int32_t  step_event_count;          // The number of step events required to complete this block
+
+  // Fields used by the motion planner to manage acceleration
+  float nominal_speed_sqr;           // Axis-limit adjusted nominal speed for this block in (mm/min)^2
+  float entry_speed_sqr;             // Entry speed at previous-current block junction in (mm/min)^2
+  float max_entry_speed_sqr;         // Maximum allowable junction entry speed in (mm/min)^2
+  float millimeters;                 // The total travel of this block in mm
+  float acceleration;                // Axes-limit adjusted line acceleration in mm/min^2
+
+  // Settings for the trapezoid generator
+  uint32_t initial_rate;              // The step rate at start of block  
+  int32_t rate_delta;                 // The steps/minute to add or subtract when changing speed (must be positive)
+  uint32_t decelerate_after;          // The index of the step event on which to start decelerating
+  uint32_t nominal_rate;              // The nominal step rate for this block in step_events/minute
+  uint32_t d_next;                    // Scaled distance to next step
+
+} block_t;
+      
+// Initialize the motion plan subsystem
+void plan_init();
+
+// 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.
+void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate);
+
+// Called when the current block is no longer needed. Discards the block and makes the memory
+// availible for new blocks.
+void plan_discard_current_block();
+
+// Gets the current block. Returns NULL if buffer empty
+block_t *plan_get_current_block();
+
+// Reset the planner position vector (in steps)
+void plan_sync_position();
+
+// Reinitialize plan with a partially completed block
+void plan_cycle_reinitialize(int32_t step_events_remaining);
+
+// Returns the status of the block ring buffer. True, if buffer is full.
+uint8_t plan_check_full_buffer();
+
+// Block until all buffered steps are executed
+void plan_synchronize();
+
+#endif

print.c

@@ -77,7 +77,7 @@ void print_uint8_base2(uint8_t n)
 		serial_write('0' + buf[i - 1]);
 }
 
-static void print_uint32_base10(unsigned long n)
+void print_uint32_base10(unsigned long n)
 { 
   unsigned char buf[10]; 
   uint8_t i = 0;

print.h

@@ -31,6 +31,8 @@ void printPgmString(const char *s);
 
 void printInteger(long n);
 
+void print_uint32_base10(uint32_t n);
+
 void print_uint8_base2(uint8_t n);
 
 void printFloat(float n);

protocol.c

@@ -104,6 +104,7 @@ ISR(PINOUT_INT_vect)
 // limit switches, or the main program.
 void protocol_execute_runtime()
 {
+  st_prep_buffer();
   if (sys.execute) { // Enter only if any bit flag is true
     uint8_t rt_exec = sys.execute; // Avoid calling volatile multiple times
     

report.c

@@ -157,9 +157,9 @@ void report_grbl_settings() {
   printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
   printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
   printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
-  printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(settings.max_travel[X_AXIS]);
+  printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printFloat(-settings.max_travel[X_AXIS]); // Grbl internally store this as negative.
-  printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(settings.max_travel[Y_AXIS]);
+  printPgmString(PSTR(" (x max travel, mm)\r\n$10=")); printFloat(-settings.max_travel[Y_AXIS]); // Grbl internally store this as negative.
-  printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(settings.max_travel[Z_AXIS]);
+  printPgmString(PSTR(" (y max travel, mm)\r\n$11=")); printFloat(-settings.max_travel[Z_AXIS]); // Grbl internally store this as negative.
   printPgmString(PSTR(" (z max travel, mm)\r\n$12=")); printInteger(settings.pulse_microseconds);
   printPgmString(PSTR(" (step pulse, usec)\r\n$13=")); printFloat(settings.default_feed_rate);
   printPgmString(PSTR(" (default feed, mm/min)\r\n$14=")); printInteger(settings.invert_mask); 

settings.c

@@ -169,9 +169,9 @@ uint8_t settings_store_global_setting(int parameter, float value) {
     case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
     case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
     case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
-    case 9: settings.max_travel[X_AXIS] = value; break;
+    case 9: settings.max_travel[X_AXIS] = -value; break;  // Store as negative for grbl internal use.
-    case 10: settings.max_travel[Y_AXIS] = value; break;
+    case 10: settings.max_travel[Y_AXIS] = -value; break; // Store as negative for grbl internal use.
-    case 11: settings.max_travel[Z_AXIS] = value; break;
+    case 11: settings.max_travel[Z_AXIS] = -value; break; // Store as negative for grbl internal use.
     case 12: 
       if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); }
       settings.pulse_microseconds = round(value); break;
@@ -206,7 +206,10 @@ uint8_t settings_store_global_setting(int parameter, float value) {
       break;
     case 24:
       if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; }
-      else { settings.flags &= ~BITFLAG_HOMING_ENABLE; }
+      else { 
+        settings.flags &= ~BITFLAG_HOMING_ENABLE; 
+        settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; 
+      }
       break;
     case 25: settings.homing_dir_mask = trunc(value); break;
     case 26: settings.homing_feed_rate = value; break;

stepper.c

@@ -19,20 +19,30 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
-   and Philipp Tiefenbacher. */
-
 #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 CRUISE_RAMP 0
+
-#define ACCEL_RAMP 1
+#define RAMP_NOOP_CRUISE 0
-#define DECEL_RAMP 2
+#define RAMP_ACCEL 1
+#define RAMP_DECEL 2
+
+#define LOAD_NOOP 0
+#define LOAD_LINE 1
+#define LOAD_BLOCK 2
+
+#define ST_NOOP 0
+#define ST_END_OF_BLOCK 1
+#define ST_DECEL 2
+#define ST_DECEL_EOB 3
+
+#define SEGMENT_BUFFER_SIZE 10
 
 // Stepper state variable. Contains running data and trapezoid variables.
 typedef struct {
@@ -40,44 +50,92 @@ typedef struct {
   int32_t counter_x,        // Counter variables for the bresenham line tracer
           counter_y, 
           counter_z;
-  uint32_t event_count;     // Total event count. Retained for feed holds. 
+  uint8_t segment_steps_remaining;  // Steps remaining in line motion
-  uint32_t step_events_remaining;  // Steps remaining in motion
 
-  // Used by Pramod Ranade inverse time algorithm
+  // Used by inverse time algorithm to track step rate
-  int32_t delta_d;     // Ranade distance traveled per interrupt tick
+  int32_t counter_d;    // Inverse time distance traveled since last step event
-  int32_t d_counter;   // Ranade distance traveled since last step event
+  uint32_t delta_d;      // Inverse time distance traveled per interrupt tick
-  uint8_t ramp_count; // Acceleration interrupt tick counter. 
+  uint32_t d_per_tick;
-  uint8_t ramp_type; // Ramp type variable.
+  
+  // 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 ramp_count;
+  uint8_t ramp_type;
 } stepper_t;
 static stepper_t st;
-static block_t *current_block;  // A pointer to the block currently being traced
 
-// Used by the stepper driver interrupt
+// Stores stepper buffer common data. Can change planner mid-block in special conditions.
-static uint8_t step_pulse_time; // Step pulse reset time after step rise
+typedef struct {  
-static uint8_t out_bits;        // The next stepping-bits to be output
+  int32_t step_events_remaining; // Tracks step event count for the executing planner block
+  uint32_t d_next;                // 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)
+  int32_t decelerate_after;
+  float mm_per_step;
+} st_data_t;
+static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
 
-// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
+// Primary stepper motion buffer
-// this blocking variable is no longer needed. Only here for safety reasons.
+typedef struct {
-static volatile uint8_t busy;   // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
+  uint8_t n_step;
+  uint8_t st_data_index;
+  uint8_t flag;
+} st_segment_t;
+static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
 
-//         __________________________
+static volatile uint8_t segment_buffer_tail;
-//        /|                        |\     _________________         ^
+static uint8_t segment_buffer_head;
-//       / |                        | \   /|               |\        |
+static uint8_t segment_next_head;
-//      /  |                        |  \ / |               | \       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.
 
+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;
+
+static plan_block_t *pl_prep_block;  // A pointer to the planner block being prepped into the stepper buffer
+static uint8_t pl_prep_index;
+static st_data_t *st_prep_data;
+static uint8_t st_data_prep_index;
+
+
+// Returns the index of the next block in the ring buffer
+// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
+static uint8_t next_block_index(uint8_t block_index) 
+{
+  block_index++;
+  if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; }
+  return(block_index);
+}
+
+static uint8_t next_block_pl_index(uint8_t block_index) 
+{
+  block_index++;
+  if (block_index == 18) { block_index = 0; }
+  return(block_index);
+}
+
+
+/*        __________________________
+         /|                        |\     _________________         ^
+        / |                        | \   /|               |\        |
+       /  |                        |  \ / |               | \       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.
@@ -91,14 +149,17 @@ void st_wake_up()
   }
   if (sys.state == STATE_CYCLE) {
     // Initialize stepper output bits
-    out_bits = settings.invert_mask; 
+    st.out_bits = settings.invert_mask; 
     // Initialize step pulse timing from settings.
-    step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
+    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
+
   }
 }
 
@@ -125,20 +186,20 @@ void st_go_idle()
 }
 
 
-// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
+/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
-// on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant
+   on the Pramod Ranade 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 any 
+   frequency and uses time-distance counters to track when its the approximate time for any 
-// step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
+   step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
-// meaning that some axes steps may not execute for a given multi-axis motion.
+   meaning that some axes steps may not execute for a given multi-axis motion.
-//   Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage
+     Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage
-// a Bresenham line algorithm for multi-axis step events which ensures the number of steps for
+   a Bresenham line algorithm for multi-axis step events which ensures the number of steps for
-// each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
+   each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
-// where one tracks time(Ranade) and the other steps.
+   where one tracks time(Ranade) and the other steps.
-//   This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
+     This interrupt pops blocks from the block_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 
+   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
+   stepper port after each pulse. The bresenham line tracer algorithm controls all three stepper
-// outputs simultaneously with these two interrupts.
+   outputs simultaneously with these two interrupts.
-//
+*/
 // NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
 // 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
 ISR(TIMER2_COMPA_vect)
@@ -150,150 +211,171 @@ ISR(TIMER2_COMPA_vect)
   // before any step pulse due to algorithm design.
   if (st.execute_step) {
     st.execute_step = false;
-    STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | out_bits;
+    STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
-    TCNT0 = step_pulse_time; // Reload Timer0 counter.
+    TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
     TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
   }
   
   busy = true;
-  sei(); // Re-enable interrupts. This ISR will still finish before returning to main program.
+  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 current block, attempt to pop one from the buffer
+  // If there is no step segment, attempt to pop one from the stepper buffer
-  if (current_block == NULL) {
+  if (st.load_flag != LOAD_NOOP) {
     
-    // Anything in the buffer? If so, initialize next motion.
+    // Anything in the buffer? If so, load and initialize next step segment.
-    current_block = plan_get_current_block();
+    if (segment_buffer_head != segment_buffer_tail) {
-    if (current_block != NULL) {
+
-      // By algorithm design, the loading of the next block never coincides with a step event,
+      // NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is 
-      // since there is always one Ranade timer tick before a step event occurs. This means
+      // a small chance that this will load at the same time as a step event. Hopefully,
-      // that the Bresenham counter math never is performed at the same time as the loading 
+      // the overhead for this loading event isn't too much.. possibly 2-5 usec.
-      // of a block, hence helping minimize total time spent in this interrupt.
+
+      // NOTE: The stepper algorithm must control the planner buffer tail as it completes
+      // the block moves. Otherwise, a feed hold can leave a few step buffer line moves 
+      // without the correct planner block information.
+      
+      st_current_segment = &segment_buffer[segment_buffer_tail];
+      
+      // Load number of steps to execute from stepper buffer
+      st.segment_steps_remaining = st_current_segment->n_step;
+    
+      // Check if the counters need to be reset for a new planner block
+      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]; //st_current_segment->st_data_index];
         
         // Initialize direction bits for block      
-      out_bits = current_block->direction_bits ^ settings.invert_mask;
+        st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
-      st.execute_step = true; // Set flag to set direction bits.
+        st.execute_step = true; // Set flag to set direction bits upon next ISR tick.
         
-      // Initialize Bresenham variables
+        // Initialize Bresenham line counters
-      st.counter_x = (current_block->step_event_count >> 1);
+        st.counter_x = (pl_current_block->step_event_count >> 1);
         st.counter_y = st.counter_x;
         st.counter_z = st.counter_x;
-      st.event_count = current_block->step_event_count;
-      st.step_events_remaining = st.event_count;
         
-      // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating.
+        // Initialize inverse time and step rate counter data
-      if (sys.state == STATE_CYCLE) {
+        st.counter_d = st_current_data->d_next;  // d_next always greater than delta_d.   
-        // Initialize Ranade variables
+                
-        st.d_counter = current_block->d_next;  
+        // During feed hold, do not update rate or ramp type. Keep decelerating.
-        st.delta_d = current_block->initial_rate;
+//         if (sys.state == STATE_CYCLE) {
-        st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
+          st.delta_d = st_current_data->initial_rate; 
+//           if (st.delta_d == st_current_data->nominal_rate) {
+//           st.ramp_type = RAMP_NOOP_CRUISE;
+          st.ramp_type = RAMP_ACCEL;
+          st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;  // Set ramp counter for trapezoid
+//           }
+//         }
+        if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
+        else { st.d_per_tick = st.delta_d; }
       
-        // Initialize ramp type.
-        if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; }
-        else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; }
-        else { st.ramp_type = ACCEL_RAMP; }
       }
 
-    } else {
-      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) {
-    // Tick acceleration ramp counter
-    st.ramp_count--;
-    if (st.ramp_count == 0) {
-      st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
-      if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
-        st.delta_d += current_block->rate_delta;
-        if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state.
-          st.ramp_type = CRUISE_RAMP;
-          st.delta_d = current_block->nominal_rate; // Set cruise velocity
-        }
-      } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration
-        if (st.delta_d > current_block->rate_delta) {
-          st.delta_d -= current_block->rate_delta;
-        } else {
-          st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
-        }
-      }
-    }
-  }
-    
-  // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event.
-  if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; }
-  else { st.d_counter -= st.delta_d; }
-  
-  // Execute Bresenham step event, when it's time to do so.
-  if (st.d_counter < 0) {  
-    st.d_counter += current_block->d_next;
-    
-    // Check for feed hold state and execute accordingly.
-    if (sys.state == STATE_HOLD) {
-      if (st.ramp_type != DECEL_RAMP) {
-        st.ramp_type = DECEL_RAMP;
-        st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
-      } 
-      if (st.delta_d <= current_block->rate_delta) {
-        st_go_idle();
-        bit_true(sys.execute,EXEC_CYCLE_STOP);
-        return;
-      }
-    }
-    
-    // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz).
-    
-    out_bits = 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 -= current_block->steps_x;
-    if (st.counter_x < 0) {
-      out_bits |= (1<<X_STEP_BIT);
-      st.counter_x += st.event_count;
-      if (out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
-      else { sys.position[X_AXIS]++; }
-    }
-    st.counter_y -= current_block->steps_y;
-    if (st.counter_y < 0) {
-      out_bits |= (1<<Y_STEP_BIT);
-      st.counter_y += st.event_count;
-      if (out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
-      else { sys.position[Y_AXIS]++; }
-    }
-    st.counter_z -= current_block->steps_z;
-    if (st.counter_z < 0) {
-      out_bits |= (1<<Z_STEP_BIT);
-      st.counter_z += st.event_count;
-      if (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_events_remaining--; // Decrement step events count
-    if (st.step_events_remaining) {
-      if (st.ramp_type != DECEL_RAMP) {
       // Acceleration and cruise handled by ramping. Just check for deceleration.
-        if (st.step_events_remaining <=  current_block->decelerate_after) {
+      if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) {
-          st.ramp_type = DECEL_RAMP;
+        if (st.ramp_type == RAMP_NOOP_CRUISE) {
-          if (st.step_events_remaining == current_block->decelerate_after) {
-            if (st.delta_d == current_block->nominal_rate) {
           st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;  // Set ramp counter for trapezoid
         } else {
           st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
         }
-          }
+        st.ramp_type = RAMP_DECEL;
-        }
-      }
-    } else {
-      // If current block is finished, reset pointer 
-      current_block = NULL;
-      plan_discard_current_block();
       }
       
-    out_bits ^= settings.invert_mask;  // Apply step port invert mask    
+      st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete.
+
+    } 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) {
+    st.ramp_count--; // Tick acceleration ramp counter
+    if (st.ramp_count == 0) { // Adjust step rate when its time
+      st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
+      if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration
+        st.delta_d += st_current_data->rate_delta;
+        if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate.
+          st.delta_d = st_current_data->nominal_rate; // Set cruising velocity
+          st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore
+        }
+      } else { // Adjust velocity for deceleration
+        if (st.delta_d > st_current_data->rate_delta) {
+          st.delta_d -= st_current_data->rate_delta;
+        } else {
+        
+          // Moving near zero feed rate. Gracefully slow down.
+          st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
+
+          // Check for and handle feed hold exit? At this point, machine is stopped.
+
+        }
+      }
+      // Finalize adjusted step rate. Ensure minimum.
+      if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
+      else { st.d_per_tick = st.delta_d; }
+    }
+  }
+  
+  // Iterate inverse time counter. Triggers each Bresenham step event.
+  st.counter_d -= st.d_per_tick; 
+  
+  // Execute Bresenham step event, when it's time to do so.
+  if (st.counter_d < 0) {  
+    st.counter_d += st_current_data->d_next; // 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 == ST_END_OF_BLOCK || st_current_segment->flag == ST_DECEL_EOB) {      
+        plan_discard_current_block();
+        st.load_flag = LOAD_BLOCK;
+      } else {
+        st.load_flag = LOAD_LINE;
+      }
+      
+      // Discard current segment
+      segment_buffer_tail = next_block_index( segment_buffer_tail );
+
+      // NOTE: sys.position updates could be done here. The bresenham counters can have 
+      // their own fast 8-bit addition-only counters. Here we would check the direction and 
+      // apply it to sys.position accordingly. However, this could take too much time.
+      
+    }
+
+    st.out_bits ^= settings.invert_mask;  // Apply step port invert mask    
   }
   busy = false;
 // SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
@@ -314,8 +396,14 @@ ISR(TIMER0_OVF_vect)
 void st_reset()
 {
   memset(&st, 0, sizeof(st));
-  current_block = NULL;
+  pl_current_block = NULL;
+  pl_prep_block = NULL;
+  pl_prep_index =   0;
+  st_data_prep_index = 0;
+  st.load_flag = LOAD_BLOCK;
   busy = false;
+  segment_buffer_tail = 0;
+  segment_next_head = 1;
 }
 
 
@@ -374,14 +462,179 @@ void st_feed_hold()
 // Only the planner de/ac-celerations profiles and stepper rates have been updated.
 void st_cycle_reinitialize()
 {
-  if (current_block != NULL) {
+//   if (pl_current_block != NULL) {
     // Replan buffer from the feed hold stop location.
-    plan_cycle_reinitialize(st.step_events_remaining);
+    
-    st.ramp_type = ACCEL_RAMP;
+    // TODO: Need to add up all of the step events in the current planner block to give 
-    st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+    // back to the planner. Should only need it for the current block. 
-    st.delta_d = 0;
+    // BUT! The planner block millimeters is all changed and may be changed into the next
-    sys.state = STATE_QUEUED;
+    // planner block. The block millimeters would need to be recalculated via step counts
-  } else {
+    // 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.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+//     st.delta_d = 0;
+//     sys.state = STATE_QUEUED;
+//   } else {
+//     sys.state = STATE_IDLE;
+//   }
     sys.state = STATE_IDLE;
+
+}
+
+
+/* Preps stepper buffer. Called from main program. 
+
+   NOTE: There doesn't seem to be a great way to figure out how many steps occur within
+   a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
+   critical problem. So, either numerical round-off checks could be made to account for 
+   them, while CPU overhead could be minimized in some way, or we can flip the algorithm
+   around to have the stepper algorithm track number of steps over an indeterminant amount
+   of time instead. 
+     In other words, we use the planner velocity floating point data to get an estimate of
+   the number of steps we want to execute. We then back out the approximate velocity for
+   the planner to use, which should be much more robust to round-off error. The main problem
+   now is that we are loading the stepper algorithm to handle acceleration now, rather than
+   pre-calculating with the main program. This approach does make sense in the way that
+   planner velocities and stepper profiles can be traced more accurately. 
+     Which is better? Very hard to tell. The time-based algorithm would be able to handle 
+   Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
+   require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
+   would occur in a non-deterministic manner.
+     I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
+
+   TODO: Need to describe the importance of continuations of step pulses between ramp states
+   and planner blocks. This has to do with Alden's problem with step "phase". The things I've
+   been doing here limit this phase issue by truncating some of the ramp timing for certain
+   events like deceleration initialization and end of block. 
+*/
+void st_prep_buffer()
+{
+  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 (pl_prep_block == NULL) {
+      pl_prep_block = plan_get_block_by_index(pl_prep_index);
+      if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
+      
+      // Prepare commonly shared planner block data for the ensuing step buffer moves
+      st_data_prep_index = next_block_index(st_data_prep_index);
+      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 new block to stepper variables.
+      // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
+      // be maintained as these execute.
+      // TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
+      // we must reload the initial rate data, such that the velocity profile is re-constructed correctly.
+      st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+      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)
+ 
+      // This data doesn't change. Could be performed in the planner, but fits nicely here.
+      // Although, acceleration can change for S-curves. So keep it here.
+      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)
+      // This definitely doesn't change, but could be precalculated in a way to help some of the 
+      // math in this handler, i.e. millimeters per step event data.
+      st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
+      st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
+              
+      // Calculate trapezoid data from planner.
+      st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index);
+      
+    }
+
+
+    /* 
+       TODO: Need to check for a planner flag to indicate a change to this planner block.
+       If so, need to check for a change in acceleration state, from deceleration to acceleration,
+       to reset the stepper ramp counters and the initial_rate data to trace the new
+       ac/de-celeration profile correctly.
+       No change conditions: 
+         - From nominal speed to acceleration from feedrate override
+         - From nominal speed to new deceleration.
+         - From acceleration to new deceleration point later or cruising point.
+         - From acceleration to immediate deceleration? Can happen during feedrate override
+           and slowing down, but likely ok by enforcing the normal ramp counter protocol.
+       Change conditions:
+         - From deceleration to acceleration, i.e. common with jogging when new blocks are added.
+    */
+
+    st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head];
+    st_prep_segment->st_data_index = st_data_prep_index;
+
+    // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'.
+    st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step );   
+//      st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
+//      st_segment->n_step = min(st_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 (st_prep_segment->n_step > st_prep_data->step_events_remaining) { 
+      st_prep_segment->n_step = st_prep_data->step_events_remaining; 
+    }
+
+    // Check if n_step exceeds decelerate point in block. Need to perform this so that the
+    // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should
+    // be OK since it is likely moving at a fast rate already.
+    if (st_prep_data->decelerate_after > 0) {
+      if (st_prep_segment->n_step > st_prep_data->decelerate_after) { 
+        st_prep_segment->n_step = st_prep_data->decelerate_after; 
+      } 
+    }
+
+//     float distance, exit_speed_sqr;
+//     distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero
+//     if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) {
+//       exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; 
+//     } else { // Acceleration or cruising ramp
+//       if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) {
+//         exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; 
+//         if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; }
+//       } else {
+//         exit_speed_sqr = pl_prep_block->nominal_speed_sqr;
+//       }
+//     }
+    
+    // Update planner block variables.
+//     pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr);
+//      pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable.
+//     pl_prep_block->millimeters -= distance; // Potential round-off error near end of block.
+//     pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter.
+        
+    // Update stepper block variables.
+    st_prep_data->step_events_remaining -= st_prep_segment->n_step;
+    if ( st_prep_data->step_events_remaining == 0 ) { 
+      // Move planner pointer to next block
+      if (st_prep_data->decelerate_after == 0) {         
+        st_prep_segment->flag = ST_DECEL_EOB;
+      } else {
+        st_prep_segment->flag = ST_END_OF_BLOCK;
+      }
+      pl_prep_index = next_block_pl_index(pl_prep_index);
+      pl_prep_block = NULL;
+printString("EOB");
+    } else {
+      if (st_prep_data->decelerate_after == 0) {         
+        st_prep_segment->flag = ST_DECEL;
+      } else {
+        st_prep_segment->flag = ST_NOOP;
+      }
+printString("x");
+    }
+      st_prep_data->decelerate_after -= st_prep_segment->n_step;
+
+    // New step block completed. Increment step buffer indices.
+    segment_buffer_head = segment_next_head;
+    segment_next_head = next_block_index(segment_buffer_head);   
+
+printInteger((long)st_prep_segment->n_step);
+printString(" ");      
+printInteger((long)st_prep_data->decelerate_after);
+printString(" ");      
+printInteger((long)st_prep_data->step_events_remaining);
   } 
 }      

stepper.h

@@ -45,4 +45,6 @@ void st_cycle_reinitialize();
 // Initiates a feed hold of the running program
 void st_feed_hold();
 
+void st_prep_buffer();
+
 #endif

stepper_new2.c

@@ -0,0 +1,601 @@
+/*
+  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 CRUISE_RAMP 0
+#define ACCEL_RAMP 1
+#define DECEL_RAMP 2
+
+#define LOAD_NOOP 0
+#define LOAD_LINE 1
+#define LOAD_BLOCK 2
+
+// 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;
+  int32_t step_events_remaining;  // Steps remaining in line motion
+
+  // Used by inverse time algorithm
+  int32_t delta_d;      // Inverse time distance traveled per interrupt tick
+  int32_t d_counter;    // Inverse time distance traveled since last step event
+  int32_t d_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;
+} stepper_t;
+static stepper_t st;
+
+#define STEPPER_BUFFER_SIZE 5
+typedef struct {
+  int32_t event_count;
+  int32_t rate;
+  uint8_t end_of_block;
+  uint8_t tick_count;
+
+  int32_t initial_rate;     // The step rate at start of block  
+  int32_t rate_delta;       // The steps/minute to add or subtract when changing speed (must be positive)
+  int32_t decelerate_after; // The index of the step event on which to start decelerating
+  int32_t nominal_rate;     // The nominal step rate for this block in step_events/minute
+  int32_t d_next;           // Scaled distance to next step
+
+} stepper_buffer_t;
+static stepper_buffer_t step_buffer[STEPPER_BUFFER_SIZE];
+
+static volatile uint8_t step_buffer_tail;
+static uint8_t step_buffer_head;
+static uint8_t step_next_head;
+
+// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
+// this blocking variable is no longer needed. Only here for safety reasons.
+static volatile uint8_t busy;   // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
+static plan_block_t *plan_current_block;  // A pointer to the planner block currently being traced
+
+
+/*        __________________________
+         /|                        |\     _________________         ^
+        / |                        | \   /|               |\        |
+       /  |                        |  \ / |               | \       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;
+    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 the Pramod Ranade 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 any 
+   step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
+   meaning that some axes steps may not execute for a given multi-axis motion.
+     Grbl's algorithm slightly differs by using a single Ranade 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, it uses a Bresenham within a Bresenham algorithm,
+   where one tracks time(Ranade) and the other steps.
+     This interrupt pops blocks from the block_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.
+*/
+// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
+// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
+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.
+  
+  // This loading step is important. Allows the stepper ISR to only handle its own variables
+  // hence no volatiles needed. Otherwise a preloading step is required by the main program
+  // or some other means to get the line motions started, and volatile would be required.
+  
+  // If there is no current block, attempt to pop one from the buffer
+  if (st.load_flag != LOAD_NOOP) {
+    
+    // Anything in the buffer? If so, initialize next motion.
+    if (step_buffer_head != step_buffer_tail) {
+
+      // NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is 
+      // a small chance that this will load at the same time as a step event. Hopefully,
+      // the overhead for this loading event isn't too much.. possibly 2-5 usec.
+
+      // NOTE: The stepper algorithm must control the planner buffer tail as it completes
+      // the block moves. Otherwise, a feed hold can leave a few step buffer line moves 
+      // without the correct planner block information.
+
+      // Load line motion from stepper buffer
+      st.step_events_remaining = step_buffer[step_buffer_tail].event_count;
+      st.delta_d = step_buffer[step_buffer_tail].rate; 
+    
+      // Check if the counters need to be reset
+      if (st.load_flag == LOAD_BLOCK) {
+        plan_current_block = plan_get_current_block();
+        
+        // Initialize direction bits for block      
+        st.out_bits = plan_current_block->direction_bits ^ settings.invert_mask;
+        st.execute_step = true; // Set flag to set direction bits.
+
+        st.counter_x = (plan_current_block->step_event_count >> 1);
+        st.counter_y = st.counter_x;
+        st.counter_z = st.counter_x;
+        
+        // This is correct. Sets the total time before the next step occurs.
+        st.counter_d = plan_current_block->d_next;  // d_next always greater than delta_d.   
+        
+        st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;  // Set ramp counter for trapezoid
+        
+      }
+      
+      st.load_flag = LOAD_NOOP; // Line motion loaded. Set no-operation flag until complete.
+
+    } 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_d -= st.delta_d; 
+  
+  // Execute Bresenham step event, when it's time to do so.
+  if (st.counter_d < 0) {  
+    st.counter_d += plan_current_block->d_next; // Reload inverse time counter
+    
+    st.out_bits = plan_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 -= plan_current_block->steps[X_AXIS];
+    if (st.counter_x < 0) {
+      st.out_bits |= (1<<X_STEP_BIT);
+      st.counter_x += plan_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 -= plan_current_block->steps[Y_AXIS];
+    if (st.counter_y < 0) {
+      st.out_bits |= (1<<Y_STEP_BIT);
+      st.counter_y += plan_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 -= plan_current_block->steps[Z_AXIS];
+    if (st.counter_z < 0) {
+      st.out_bits |= (1<<Z_STEP_BIT);
+      st.counter_z += plan_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_events_remaining--; // Decrement step events count
+    
+    // Tracking step events may not be the right thing. Need to track time instead.
+    // If a step completes and the inverse counters are reset, then the line motion time
+    // to execute gets truncated. Thus, screwing up the acceleration.
+    // Time can change at the first block, a transition point or end of block. It is not constant.
+    
+    // Time must be truncated when the last step in the block is executed. Ensures there are 
+    // no step phase issues with the following block. Or, the ISR timer count must be computed
+    // to be exact, such that the last step occurs correctly.
+
+    // This should always/automatically occur for a true trapezoid. The first acceleration
+    // should be 1/2 accel_tick and the last decel should also. The difference is taken up by
+    // the transition from cruise to decel.
+
+    // For a cruising entry, the transition block takes care of incompatibility.
+    // For a cruising exit, the exit block needs to truncate. Especially when the cruising
+    // continues onward to the next block.
+    
+    if (st.step_events_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 (step_buffer[step_buffer_tail].end_of_block) {      
+        plan_discard_current_block();
+        st.load_flag = LOAD_BLOCK;
+      } else {
+        st.load_flag = LOAD_LINE;
+      }
+      
+      // Discard current block
+      if (step_buffer_head != step_buffer_tail) { 
+        step_buffer_tail = next_block_index( step_buffer_tail );
+      }
+
+      // NOTE: sys.position updates could be done here. The bresenham counters can have 
+      // their own incremental counters. Here we would check the direction and apply it
+      // to sys.position accordingly. However, this could take some time.
+      
+    }
+
+    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;
+}
+
+
+// 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_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 (current_block != NULL) {
+    // Replan buffer from the feed hold stop location.
+    plan_cycle_reinitialize(st.step_events_remaining);
+    st.ramp_type = ACCEL_RAMP;
+    st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+    st.delta_d = 0;
+    sys.state = STATE_QUEUED;
+  } else {
+    sys.state = STATE_IDLE;
+  }
+}
+
+
+// Preps stepper buffer. Called from main program. Sends short line segments of constant
+// velocity to the stepper driver. Ac/de-celeration calculations are performed here.
+// NOTE: Line motions can range from 1-300
+// TODO: Could be very easy to install adaptive Bresenham resolution with this prep function.\
+         That is, if the acceleration calculations are performed here rather than in the ISR.
+         
+void st_prep_buffer()
+{
+   while (st.buffer_tail != st.next_head) { // Check if we need to fill the buffer.
+
+     step_block_t *step_block = &step_buffer[st.buffer_head];
+
+     // Determine if we need to load a new planner block.
+     if (plan_current_block == NULL) {
+       plan_current_block = plan_get_current_block();
+       if (plan_current_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
+              
+       // Initialize Bresenham variables
+       step_buffer[st.buffer_head] = plan_current_block->step_event_count;
+       //st.step_events_remaining = st.event_count;
+       
+       // Convert new block to stepper variables.
+       // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
+       // be maintained as these execute.
+       // TODO: The initial rate needs to be sent back to the planner to update the entry speed
+       block->initial_rate = ceil(sqrt(plan_current_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+       block->nominal_rate = ceil(plan_current_block->nominal_speed*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+
+       // This data doesn't change. Could be performed in the planner, but fits nicely here.
+       // Although, acceleration can change for S-curves. So keep it here.
+       block->rate_delta = ceil(plan_current_block->acceleration*
+         ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
+       // This definitely doesn't change, but could be precalculated in a way to help some of the 
+       // math in this handler, i.e. millimeters per step event data.
+       block->d_next = ceil((plan_current_block->millimeters*INV_TIME_MULTIPLIER)/plan_current_block->step_event_count); // (mult*mm/step)
+       
+       // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating.
+       if (sys.state == STATE_CYCLE) {       }
+     }
+ 
+ 
+// Track instead the trapezoid line and use the average of the entry and exit velocities
+// to determine step rate. This should take care of the deceleration issue automatically... 
+// i think.
+// First need to figure out what type of profile is segment is, i.e. acceleration only, accel
+// to decel triangle, cruise to decel, or all three. May need more profile data to compute this
+// from the planner itself, like accelerate until.
+// Another issue. This is only tracking the velocity profile, not the distance covered over that
+// time period. This can lead to an unsynchronized velocity profile and steps executed. But,
+// how much drift is there really? Enough to be a problem? Not sure. I would think typical 
+// drift would be on the order of a few steps, more depending on step resolution. 
+entry_rate = last_exit_rate;
+time = 250 ISR ticks per acceleration tick.
+distance = 0;
+if (distance_traveled < accelerate_until)
+  exit_rate = entry_rate + acceleration*time;
+  if (exit_rate > nominal_rate) {
+    exit_rate = nominal_rate;
+    time = 2*(accelerate_until-distance_travel)/(entry_rate+nominal_rate);
+    // distance = accelerate_until; // Enforce distance?
+    // Truncate this segment.
+  }
+} else if (distance_traveled >= decelerate_after) {
+  if (accelerate_until == decelerate_after) {
+    time = last time;
+    exit_rate = entry_rate;
+  } else {
+    exit_rate = entry_rate - acceleration*time;
+  }
+} else {  
+  exit_rate = nominal_rate; // Just cruise
+  distance = nominal_rate*time;
+  if (distance > decelerate_after) { // Truncate segment at nominal rate.
+    time = (decelerate_after-distance_traveled)/(nominal_rate);
+    distance = decelerate_after;
+  }
+}
+mean_rate = 0.5*(entry_rate+exit_rate);
+distance = mean_rate*time;
+  
+   
+if (entry_rate < nominal_rate) {  
+  if (entry_distance < decelerate_after) { // Acceleration case
+    exit_rate = entry_rate + acceleration*time
+    exit_rate = min(exit_rate,nominal_rate);
+mean_rate = 0.5*(entry_rate + exit_rate);
+distance = mean_rate*time;
+if (distance > decelerate_after) {
+  exit_rate = 
+
+ 
+ // If the MINIMUM_STEP_RATE is less than ACCELERATION_TICKS_PER_SECOND then there can be
+ // rate adjustements that have less than one step per tick.
+ // How do you deal with the remainer?
+time = 250 ISR ticks per acceleration tick. (30000/120)
+delta_d*time // mm per acceleration tick
+delta_d*time/d_next // number of steps/acceleration_tick. Chance of integer overflow.
+delta_d*time/d_next + last_remainder. // steps/acceleration_tick.
+n_step*d_next/delta_d // number of ISR ticks for enforced n_steps.
+
+// In floating point? Then convert?
+// Requires exact millimeters. Roundoff might be a problem. But could be corrected by just
+// checking if the total step event counts are performed.
+// Could be limited by float conversion and about 1e7 steps per block.
+line_mm = feed_rate / acc_tick // mm per acc_tick
+n_steps = floor(line_mm * step_event_remaining/millimeters_remaining) // steps. float 7.2 digits|int32 10 digits
+millimeters_remaining -= line_mm; 
+step_events_remaining -= n_steps;
+
+// There doesn't seem to be a way to avoid this divide here. 
+line_mm = feed_rate / acc_tick // mm per acc_tick
+n_steps = floor( (line_mm+line_remainder) * step_event_count/millimeters) // steps. float 7.2 digits|int32 10 digits
+line_remainder = line_mm - n_steps*(millimeters/step_event_count);
+
+// Need to handle when rate is very very low, i.e. less than one step per accel tick.
+// Could be bounded by MINIMUM_STEP_RATE.
+
+// 1. Figure out how many steps occur exactly within n ISR ticks.
+// 2. Account for step-time remainder for next line motion exactly.
+// 3. At the end of block, determine exact number of ISR ticks to finish the steps. Or,\
+       have the ISR track steps to exit on time. It would require an extra counter.
+
+// NOTE: There doesn't seem to be a great way to figure out how many steps occur within
+// a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
+// critical problem. So, either numerical round-off checks could be made to account for 
+// them, while CPU overhead could be minimized in some way, or we can flip the algorithm
+// around to have the stepper algorithm track number of steps over an indeterminant amount
+// of time instead. 
+//   In other words, we use the planner velocity floating point data to get an estimate of
+// the number of steps we want to execute. We then back out the approximate velocity for
+// the planner to use, which should be much more robust to round-off error. The main problem
+// now is that we are loading the stepper algorithm to handle acceleration now, rather than
+// pre-calculating with the main program. This approach does make sense in the way that
+// planner velocities and stepper profiles can be traced more accurately. 
+//   Which is better? Very hard to tell. The time-based algorithm would be able to handle 
+// Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
+// require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
+// would occur in a non-deterministic manner.
+//   I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
+
+feed_rate/120 = millimeters per acceleration tick
+
+
+     steps?
+     d_next // (mult*mm/step)
+     rate   // (mult*mm/isr_tic)
+     rate/d_next  // step/isr_tic
+
+     if (plan_current_block->step_events_remaining <= plan_current_block->decelerate_after) {
+       // Determine line segment velocity and associated inverse time counter.
+       if (step_block.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
+         step_block.delta_d += plan_current_block->rate_delta;
+         if (step_block.delta_d >= plan_current_block->nominal_rate) { // Reached cruise state.
+           step_block.ramp_type = CRUISE_RAMP;
+           step_block.delta_d = plan_current_block->nominal_rate; // Set cruise velocity
+         }
+       } 
+     } else { // Adjust velocity for deceleration
+       if (step_block.delta_d > plan_current_block->rate_delta) {
+         step_block.delta_d -= plan_current_block->rate_delta;
+       } else {
+         step_block.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
+       }
+     }
+      
+     // Incorrect. Can't overwrite delta_d. Needs to override instead.
+     if (step_block.delta_d < MINIMUM_STEP_RATE) { step_block.delta_d = MINIMUM_STEP_RATE; }
+
+     /* - Compute the number of steps needed to complete this move over the move time, i.e.
+          ISR_TICKS_PER_ACCELERATION_TICK.
+          - The first block in the buffer is half of the move time due to midpoint rule.
+        - Check if this reaches the deceleration after location. If so, truncate move. Also,
+          if this is a triangle move, double the truncated move to stay with midpoint rule.
+          NOTE: This can create a stepper buffer move down to just one step in length. 
+        - Update the planner block entry speed for the planner to compute from end of the
+          stepper buffer location.
+        - If a feed hold occurs, begin to enforce deceleration, while enforcing the above rules.
+          When the deceleration is complete, all we need to do is update the planner block
+          entry speed and force a replan.
+     */    
+ 
+     // Planner block move completed.
+     // TODO: planner buffer tail no longer needs to be volatile. only accessed by main program.
+     if (st.step_events_remaining == 0) { 
+       plan_current_block = NULL; // Set flag that we are done with this planner block.
+       plan_discard_current_block();
+     }
+     
+
+  
+  
+     
+     step_buffer_head = step_next_head;
+     step_next_head = next_block_index(step_buffer_head); 
+      
+   }      
+}      

stepper_new_dual_ISR.c

@@ -34,9 +34,7 @@
 // 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
+  int32_t counter[N_AXIS];  // Counter variables for the bresenham line tracer
-          counter_y, 
-          counter_z;
   uint32_t event_count;     // Total event count. Retained for feed holds. 
   uint32_t step_events_remaining;  // Steps remaining in motion
 
@@ -182,26 +180,29 @@ won't take too much time in the interrupt.
   // Prepare Bresenham step event, when it's time to do so.
   if (st.d_counter < 0) {  
     st.d_counter += current_block->d_next;
-      
-    // Load next step
-    out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
     st.execute_step = true;
       
+    // Configure next step
+    out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
+    
     // Execute step displacement profile by Bresenham line algorithm
-    st.counter_x -= current_block->steps_x;
+    st.counter[X_AXIS] -= current_block->steps_x; // Doesn't change when set up.
-    if (st.counter_x < 0) {
+    if (st.counter[X_AXIS] < 0) {
       out_bits |= (1<<X_STEP_BIT);
-      st.counter_x += st.event_count;
+      st.counter[X_AXIS] += st.event_count;
+      st.n_step[X_AXIS]; // Track number of steps
     }
-    st.counter_y -= current_block->steps_y;
+    st.counter[Y_AXIS] -= current_block->steps_y;
-    if (st.counter_y < 0) {
+    if (st.counter[Y_AXIS] < 0) {
       out_bits |= (1<<Y_STEP_BIT);
-      st.counter_y += st.event_count;
+      st.counter[Y_AXIS] += st.event_count;
+      st.n_step[Y_AXIS]++;
     }
-    st.counter_z -= current_block->steps_z;
+    st.counter[Z_AXIS] -= current_block->steps_z;
-    if (st.counter_z < 0) {
+    if (st.counter[Z_AXIS] < 0) {
       out_bits |= (1<<Z_STEP_BIT);
-      st.counter_z += st.event_count;
+      st.counter[Z_AXIS] += st.event_count;
+      st.n_step[Z_AXIS]++;      
     }
 
     // Check step events for trapezoid change or end of block.

stepper_new_time.c

@@ -0,0 +1,680 @@
+/*
+  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_LINE 1
+#define LOAD_BLOCK 2
+
+#define ST_NOOP 0
+#define ST_END_OF_BLOCK 1
+
+// 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;
+  int8_t segment_steps_remaining;  // Steps remaining in line motion
+
+  // Used by inverse time algorithm to track step rate
+  int32_t counter_d;    // Inverse time distance traveled since last step event
+  int32_t delta_d;      // Inverse time distance traveled per interrupt tick
+  int32_t d_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 ramp_count;
+  uint8_t ramp_type;
+} stepper_t;
+static stepper_t st;
+
+#define SEGMENT_BUFFER_SIZE 5
+// Stores stepper buffer common data. Can change planner mid-block in special conditions.
+typedef struct {  
+  int32_t step_events_remaining; // Tracks step event count for the executing planner block
+  int32_t d_next;                // Scaled distance to next step
+  float mm_per_step;
+} st_data_t;
+static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
+
+// Primary stepper motion buffer
+typedef struct {
+  uint8_t n_step;
+  int32_t rate;
+  uint8_t st_data_index;
+  uint8_t flag;
+} st_segment_t;
+static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
+
+static volatile uint8_t segment_buffer_tail;
+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.
+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;
+
+static plan_block_t *pl_prep_block;  // A pointer to the planner block being prepped into the stepper buffer
+static uint8_t pl_prep_index;
+static st_data_t *st_prep_data;
+static uint8_t st_data_prep_index;
+
+
+// Returns the index of the next block in the ring buffer
+// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
+static uint8_t next_block_index(uint8_t block_index) 
+{
+  block_index++;
+  if (block_index == SEGMENT_BUFFER_SIZE) { block_index = 0; }
+  return(block_index);
+}
+
+
+/*        __________________________
+         /|                        |\     _________________         ^
+        / |                        | \   /|               |\        |
+       /  |                        |  \ / |               | \       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;
+    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 the Pramod Ranade 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 any 
+   step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
+   meaning that some axes steps may not execute for a given multi-axis motion.
+     Grbl's algorithm slightly differs by using a single Ranade 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, it uses a Bresenham within a Bresenham algorithm,
+   where one tracks time(Ranade) and the other steps.
+     This interrupt pops blocks from the block_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.
+*/
+// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
+// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
+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) {
+
+      // NOTE: Loads after a step event. At high rates above 1/2 ISR frequency, there is 
+      // a small chance that this will load at the same time as a step event. Hopefully,
+      // the overhead for this loading event isn't too much.. possibly 2-5 usec.
+
+      // NOTE: The stepper algorithm must control the planner buffer tail as it completes
+      // the block moves. Otherwise, a feed hold can leave a few step buffer line moves 
+      // without the correct planner block information.
+      
+      st_current_segment = &segment_buffer[segment_buffer_tail];
+      
+      // Load number of steps to execute from stepper buffer
+      st.segment_steps_remaining = st_current_segment->n_step;
+      st.delta_d = st_current_segment->rate;
+      
+      // Check if the counters need to be reset for a new planner block
+      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[st_current_segment->st_data_index];
+        
+        // Initialize direction bits for block      
+        st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
+        st.execute_step = true; // Set flag to set direction bits upon next ISR tick.
+        
+        // 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 and step rate counter data
+        st.counter_d = st_current_data->d_next;  // d_next always greater than delta_d.   
+      }
+      st.load_flag = LOAD_NOOP; // Motion loaded. Set no-operation flag until complete.
+
+    } 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_d -= st.delta_d; 
+  
+  // Execute Bresenham step event, when it's time to do so.
+  if (st.counter_d < 0) {  
+    st.counter_d += st_current_data->d_next; // 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 == ST_END_OF_BLOCK) {      
+        plan_discard_current_block();
+        st.load_flag = LOAD_BLOCK;
+      } else {
+        st.load_flag = LOAD_LINE;
+      }
+      
+      // Discard current block
+      if (segment_buffer_head != segment_buffer_tail) { 
+        segment_buffer_tail = next_block_index( segment_buffer_tail );
+      }
+
+      // NOTE: sys.position updates could be done here. The bresenham counters can have 
+      // their own fast 8-bit addition-only counters. Here we would check the direction and 
+      // apply it to sys.position accordingly. However, this could take too much time.
+      
+    }
+
+    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));
+  pl_current_block = NULL;
+  pl_prep_block = NULL;
+  pl_prep_index =   0;
+  st_data_prep_index = 0;
+  st.load_flag = LOAD_BLOCK;
+  busy = false;
+  segment_buffer_tail = 0;
+  segment_next_head = 1;
+}
+
+
+// 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_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.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+    st.delta_d = 0;
+    sys.state = STATE_QUEUED;
+  } else {
+    sys.state = STATE_IDLE;
+  }
+}
+
+
+/* Preps stepper buffer. Called from main program. 
+
+   NOTE: There doesn't seem to be a great way to figure out how many steps occur within
+   a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
+   critical problem. So, either numerical round-off checks could be made to account for 
+   them, while CPU overhead could be minimized in some way, or we can flip the algorithm
+   around to have the stepper algorithm track number of steps over an indeterminant amount
+   of time instead. 
+     In other words, we use the planner velocity floating point data to get an estimate of
+   the number of steps we want to execute. We then back out the approximate velocity for
+   the planner to use, which should be much more robust to round-off error. The main problem
+   now is that we are loading the stepper algorithm to handle acceleration now, rather than
+   pre-calculating with the main program. This approach does make sense in the way that
+   planner velocities and stepper profiles can be traced more accurately. 
+     Which is better? Very hard to tell. The time-based algorithm would be able to handle 
+   Bresenham step adaptive-resolution much easier and cleaner. Whereas, the step-based would
+   require some additional math in the stepper algorithm to adjust on the fly, plus adaptation
+   would occur in a non-deterministic manner.
+     I suppose it wouldn't hurt to build both to see what's better. Just a lot more work.
+
+   TODO: Need to describe the importance of continuations of step pulses between ramp states
+   and planner blocks. This has to do with Alden's problem with step "phase". The things I've
+   been doing here limit this phase issue by truncating some of the ramp timing for certain
+   events like deceleration initialization and end of block. 
+*/
+void st_prep_buffer()
+{
+  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 (pl_prep_block == NULL) {
+      pl_prep_block = plan_get_block_by_index(pl_prep_index);
+      if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
+      
+      // Prepare commonly shared planner block data for the ensuing step buffer moves
+      st_data_prep_index = next_block_index(st_data_prep_index);
+      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 new block to stepper variables.
+      // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
+      // be maintained as these execute.
+      // TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
+      // we must reload the initial rate data, such that the velocity profile is re-constructed correctly.
+//       st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+//       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)
+ 
+      // This data doesn't change. Could be performed in the planner, but fits nicely here.
+      // Although, acceleration can change for S-curves. So keep it here.
+//       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)
+      // This definitely doesn't change, but could be precalculated in a way to help some of the 
+      // math in this handler, i.e. millimeters per step event data.
+      st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
+      
+      st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
+    }
+
+
+    /*
+      // Check if planner has changed block exit parameters. If so, then we have to update the 
+  // velocity profile for the remainder of this block. Otherwise, the block hasn't changed.
+  if (exit_speed_sqr != last_exit_speed_sqr) { 
+    intersect_distance = 0.5*( millimeters + (entry_speed_sqr-exit_speed_sqr)/(2*acceleration) );  
+    if (intersect_distance <= 0) { // Deceleration only.
+      final_rate_sqr = initial_rate_sqr - 2*acceleration*segment_distance;
+      block->decelerate_after = 0; 
+    } else {
+      decelerate_after = (block->nominal_speed_sqr - exit_speed_sqr)/(2*block->acceleration);
+      if (decelerate_after > intersect_distance) { decelerate_after = intersect_distance; }
+      if (decelerate_after > block->millimeters) { decelerate_after = block->millimeters; }
+    }
+  }
+  
+  
+  
+  
+  
+    n_step = 100; // Estimate distance we're going to travel in this segment
+    if (n_step > step_events_remaining) { n_step = step_events_remaining; };
+    
+    segment_distance = n_step*mm_per_step; // True distance traveled  
+    
+    // ISSUE: Either weighted average speed or truncated segments must be used. Otherwise
+    // accelerations, in particular high value, will not perform correctly.
+    if (distance_traveled < decelerate_after) {
+      if (segment_distance + distance_traveled > decelerate_after) {
+        n_step = ceil((decelerate_after-distance_traveled)/mm_per_step);
+        segment_distance = n_step*mm_per_step;
+      }
+    }
+    
+    
+    
+    
+    // based on time?
+    time = incremental time;
+    v_exit = v_entry + acceleration*time;
+    if (v_exit > v_nominal) {
+      time_accel = (v_nominal-v_entry)/acceleration;
+      distance = v_entry*time_accel + 0.5*acceleration*time_accel**2;
+      time_remaining = time - time_accel;
+    }
+    distance = v_entry*time + 0.5*acceleration*time*time;
+    
+    
+    t_accel = (v_nominal - v_entry) / acceleration;
+    t_decel = (v_exit - v_nominal) / -acceleration;
+    dt = (v_entry-v_exit)/acceleration;
+    if 
+    
+    
+    
+    
+    // What's the speed of this segment? Computing per segment can get expensive, but this
+    // would allow S-curves to be installed pretty easily here.
+    if (initial_speed < nominal_speed) {
+      if (initial_distance < decelerate_after) { // Acceleration
+        exit_speed = sqrt(initial_speed*initial_speed + 2*acceleration*distance);
+        if (exit_speed > nominal_speed) { exit_speed = nominal_speed; }
+      } else { // Deceleration
+        exit_speed = sqrt(initial_speed*initial_speed - 2*acceleration*distance);
+      }
+      average_speed = 0.5*(initial_speed + exit_speed);
+    } else { // Cruise
+      average_speed = nominal_speed;
+    }
+    segment_rate = ceil(average_speed*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+    if (segment_rate < MINIMUM_STEP_RATE) { segment_rate = MINIMUM_STEP_RATE; }
+    
+    
+
+  
+    */
+
+
+
+    /* 
+       TODO: Need to check for a planner flag to indicate a change to this planner block.
+       If so, need to check for a change in acceleration state, from deceleration to acceleration,
+       to reset the stepper ramp counters and the initial_rate data to trace the new
+       ac/de-celeration profile correctly.
+       No change conditions: 
+         - From nominal speed to acceleration from feedrate override
+         - From nominal speed to new deceleration.
+         - From acceleration to new deceleration point later or cruising point.
+         - From acceleration to immediate deceleration? Can happen during feedrate override
+           and slowing down, but likely ok by enforcing the normal ramp counter protocol.
+       Change conditions:
+         - From deceleration to acceleration, i.e. common with jogging when new blocks are added.
+    */
+
+    st_segment_t *st_prep_block = &segment_buffer[segment_buffer_head];
+    st_prep_block->st_data_index = st_data_prep_index;
+
+    // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'.
+    st_prep_block->n_step = 100; //floor( (exit_speed*approx_time)/mm_per_step );   
+//      st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
+//      st_segment->n_step = min(st_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 (st_prep_block->n_step > st_prep_data->step_events_remaining) { 
+      st_prep_block->n_step = st_prep_data->step_events_remaining; 
+    }
+
+    // Check if n_step exceeds decelerate point in block. Need to perform this so that the
+    // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should
+    // be OK since it is likely moving at a fast rate already.
+    if (st_prep_block->n_step > pl_prep_block->decelerate_after) { 
+      st_prep_block->n_step = pl_prep_block->decelerate_after; 
+    } 
+    
+    float distance, exit_speed_sqr;
+    distance = st_prep_block->n_step*st_prep_data->mm_per_step; // Always greater than zero
+    if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) {
+      exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; 
+      // Set ISR tick reset flag for deceleration ramp.
+    } else { // Acceleration or cruising ramp
+      if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) {
+        exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; 
+        if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; }
+      } else {
+        exit_speed_sqr = pl_prep_block->nominal_speed_sqr;
+      }
+    }
+    
+    
+      // Adjust inverse time counter for ac/de-celerations
+  if (st.ramp_type) {
+    st.ramp_count--; // Tick acceleration ramp counter
+    if (st.ramp_count == 0) { // Adjust step rate when its time
+      st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
+      if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration
+        st.delta_d += st_current_data->rate_delta;
+        if (st.delta_d >= st_current_data->nominal_rate) { // Reached nominal rate.
+          st.delta_d = st_current_data->nominal_rate; // Set cruising velocity
+          st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to ignore
+        }
+      } else { // Adjust velocity for deceleration
+        if (st.delta_d > st_current_data->rate_delta) {
+          st.delta_d -= st_current_data->rate_delta;
+        } else {
+        
+          // Moving near zero feed rate. Gracefully slow down.
+          st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
+
+          // Check for and handle feed hold exit? At this point, machine is stopped.
+
+        }
+      }
+      // Finalize adjusted step rate. Ensure minimum.
+      if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
+      else { st.d_per_tick = st.delta_d; }
+    }
+  }
+           // During feed hold, do not update rate or ramp type. Keep decelerating.
+        if (sys.state == STATE_CYCLE) {
+          st.delta_d = st_current_data->initial_rate; 
+          if (st.delta_d == st_current_data->nominal_rate) {
+            ramp_type = RAMP_NOOP_CRUISE;
+          st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;  // Set ramp counter for trapezoid
+        } 
+          // Acceleration and cruise handled by ramping. Just check for deceleration.
+      if (st_current_segment->flag == ST_NOOP) {
+        if (st.ramp_type == RAMP_NOOP_CRUISE) {
+          st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;  // Set ramp counter for trapezoid
+        } else {
+          st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
+        }
+        st.ramp_type = RAMP_DECEL;
+      }
+      
+    
+    
+    
+    
+    // Update planner block variables.
+    pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr);
+//      pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable.
+    pl_prep_block->millimeters -= distance; // Potential round-off error near end of block.
+    pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter.
+    
+    // Update stepper block variables.
+    st_prep_data->step_events_remaining -= st_prep_block->n_step;
+    if ( st_prep_data->step_events_remaining == 0 ) { 
+      // Move planner pointer to next block
+      st_prep_block->flag = ST_END_OF_BLOCK;
+      pl_prep_index = next_block_index(pl_prep_index);
+      pl_prep_block = NULL;
+    } else {
+      st_prep_block->flag = ST_NOOP;
+    }
+    
+    // New step block completed. Increment step buffer indices.
+    segment_buffer_head = segment_next_head;
+    segment_next_head = next_block_index(segment_buffer_head); 
+     
+  }      
+}      

stepper_old.c

@@ -0,0 +1,387 @@
+/*
+  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/>.
+*/
+
+/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
+   and Philipp Tiefenbacher. */
+
+#include <avr/interrupt.h>
+#include "stepper.h"
+#include "config.h"
+#include "settings.h"
+#include "planner.h"
+
+// Some useful constants
+#define TICKS_PER_MICROSECOND (F_CPU/1000000)
+#define CRUISE_RAMP 0
+#define ACCEL_RAMP 1
+#define DECEL_RAMP 2
+
+// 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;
+  int32_t event_count;     // Total event count. Retained for feed holds. 
+  int32_t step_events_remaining;  // Steps remaining in motion
+
+  // Used by Pramod Ranade inverse time algorithm
+  int32_t delta_d;     // Ranade distance traveled per interrupt tick
+  int32_t d_counter;   // Ranade distance traveled since last step event
+  uint8_t ramp_count; // Acceleration interrupt tick counter. 
+  uint8_t ramp_type; // Ramp type variable.
+  uint8_t execute_step; // Flags step execution for each interrupt.
+  
+} stepper_t;
+static stepper_t st;
+static block_t *current_block;  // A pointer to the block currently being traced
+
+// Used by the stepper driver interrupt
+static uint8_t step_pulse_time; // Step pulse reset time after step rise
+static uint8_t out_bits;        // The next stepping-bits to be output
+
+// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
+// this blocking variable is no longer needed. Only here for safety reasons.
+static volatile uint8_t busy;   // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
+
+//         __________________________
+//        /|                        |\     _________________         ^
+//       / |                        | \   /|               |\        |
+//      /  |                        |  \ / |               | \       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
+    out_bits = settings.invert_mask; 
+    // Initialize step pulse timing from settings.
+    step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
+    // Enable stepper driver interrupt
+    st.execute_step = false;
+    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 the Pramod Ranade 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 any 
+// step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
+// meaning that some axes steps may not execute for a given multi-axis motion.
+//   Grbl's algorithm slightly differs by using a single Ranade 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, it uses a Bresenham within a Bresenham algorithm,
+// where one tracks time(Ranade) and the other steps.
+//   This interrupt pops blocks from the block_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.
+//
+// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
+// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
+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) ) | out_bits;
+    TCNT0 = step_pulse_time; // Reload Timer0 counter.
+    TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
+  }
+  
+  busy = true;
+  sei(); // Re-enable interrupts. This ISR will still finish before returning to main program.
+  
+  // If there is no current block, attempt to pop one from the buffer
+  if (current_block == NULL) {
+  
+    // Anything in the buffer? If so, initialize next motion.
+    current_block = plan_get_current_block();
+    if (current_block != NULL) {
+      // By algorithm design, the loading of the next block never coincides with a step event,
+      // since there is always one Ranade timer tick before a step event occurs. This means
+      // that the Bresenham counter math never is performed at the same time as the loading 
+      // of a block, hence helping minimize total time spent in this interrupt.
+
+      // Initialize direction bits for block      
+      out_bits = current_block->direction_bits ^ settings.invert_mask;
+      st.execute_step = true; // Set flag to set direction bits.
+      
+      // Initialize Bresenham variables
+      st.counter_x = (current_block->step_event_count >> 1);
+      st.counter_y = st.counter_x;
+      st.counter_z = st.counter_x;
+      st.event_count = current_block->step_event_count;
+      st.step_events_remaining = st.event_count;
+      
+      // During feed hold, do not update Ranade counter, rate, or ramp type. Keep decelerating.
+      if (sys.state == STATE_CYCLE) {
+        // Initialize Ranade variables
+        st.d_counter = current_block->d_next;  
+        st.delta_d = current_block->initial_rate;
+        st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
+        
+        // Initialize ramp type.
+        if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; }
+        else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; }
+        else { st.ramp_type = ACCEL_RAMP; }
+      }
+      
+    } else {
+      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) {
+    // Tick acceleration ramp counter
+    st.ramp_count--;
+    if (st.ramp_count == 0) {
+      st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
+      if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
+        st.delta_d += current_block->rate_delta;
+        if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state.
+          st.ramp_type = CRUISE_RAMP;
+          st.delta_d = current_block->nominal_rate; // Set cruise velocity
+        }
+      } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration
+        if (st.delta_d > current_block->rate_delta) {
+          st.delta_d -= current_block->rate_delta;
+        } else {
+          st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
+        }
+      }
+    }
+  }
+    
+  // Iterate Pramod Ranade inverse time counter. Triggers each Bresenham step event.
+  if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; }
+  else { st.d_counter -= st.delta_d; }
+  
+  // Execute Bresenham step event, when it's time to do so.
+  if (st.d_counter < 0) {  
+    st.d_counter += current_block->d_next;
+    
+    // Check for feed hold state and execute accordingly.
+    if (sys.state == STATE_HOLD) {
+      if (st.ramp_type != DECEL_RAMP) {
+        st.ramp_type = DECEL_RAMP;
+        st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+      } 
+      if (st.delta_d <= current_block->rate_delta) {
+        st_go_idle();
+        bit_true(sys.execute,EXEC_CYCLE_STOP);
+        return;
+      }
+    }
+    
+    // TODO: Vary Bresenham resolution for smoother motions or enable faster step rates (>20kHz).
+    
+    out_bits = 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 -= current_block->steps[X_AXIS];
+    if (st.counter_x < 0) {
+      out_bits |= (1<<X_STEP_BIT);
+      st.counter_x += st.event_count;
+      if (out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
+      else { sys.position[X_AXIS]++; }
+    }
+    st.counter_y -= current_block->steps[Y_AXIS];
+    if (st.counter_y < 0) {
+      out_bits |= (1<<Y_STEP_BIT);
+      st.counter_y += st.event_count;
+      if (out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
+      else { sys.position[Y_AXIS]++; }
+    }
+    st.counter_z -= current_block->steps[Z_AXIS];
+    if (st.counter_z < 0) {
+      out_bits |= (1<<Z_STEP_BIT);
+      st.counter_z += st.event_count;
+      if (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_events_remaining--; // Decrement step events count
+    if (st.step_events_remaining) {
+      if (st.ramp_type != DECEL_RAMP) {
+        // Acceleration and cruise handled by ramping. Just check for deceleration.
+        if (st.step_events_remaining <=  current_block->decelerate_after) {
+          st.ramp_type = DECEL_RAMP;
+          if (st.step_events_remaining == current_block->decelerate_after) {
+            if (st.delta_d == current_block->nominal_rate) {
+              st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;  // Set ramp counter for trapezoid
+            } else {
+              st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
+            }
+          }
+        }
+      }
+    } else {
+      // If current block is finished, reset pointer 
+      current_block = NULL;
+      plan_discard_current_block();
+    }
+
+    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));
+  current_block = NULL;
+  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_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 (current_block != NULL) {
+    // Replan buffer from the feed hold stop location.
+    plan_cycle_reinitialize(st.step_events_remaining);
+    st.ramp_type = ACCEL_RAMP;
+    st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+    st.delta_d = 0;
+    sys.state = STATE_QUEUED;
+  } else {
+    sys.state = STATE_IDLE;
+  }
+}