Initial v0.8 ALPHA commit. Features multi-tasking run-time command execution (feed hold, cycle start, reset, status query). Extensive re-structuring of code for future features.

- ALPHA status. - Multitasking ability with run-time command executions for real-time control and feedback. - Decelerating feed hold and resume during operation. - System abort/reset, which immediately kills all movement and re-initializes grbl. - Re-structured grbl to easily allow for new features: Status reporting, jogging, backlash compensation. (To be completed in the following releases.) - Resized TX/RX serial buffers (32/128 bytes) - Increased planner buffer size to 20 blocks. - Updated documentation.
2011-12-08 18:47:48 -07:00 · 2011-12-08 18:47:48 -07:00 · 03e2ca7cd5
commit 03e2ca7cd5
parent 292fcca67f
23 changed files with 841 additions and 477 deletions
--- a/config.h
+++ b/config.h
@ -26,9 +26,7 @@
 #define BAUD_RATE 9600
-// Updated default pin-assignments from 0.6 onwards 
+// Define pin-assignments
 // (see bottom of file for a copy of the old config)
 #define STEPPERS_DISABLE_DDR     DDRB
 #define STEPPERS_DISABLE_PORT    PORTB
 #define STEPPERS_DISABLE_BIT         0
@ -56,6 +54,16 @@
 #define SPINDLE_DIRECTION_PORT PORTB
 #define SPINDLE_DIRECTION_BIT 5
 // Define runtime command special characters. These characters are 'picked-off' directly from the
 // serial read data stream and are not passed to the grbl line execution parser. Select characters
 // that do not and must not exist in the streamed g-code program. ASCII control characters may be 
 // used, if they are available per user setup.
 // TODO: Solidify these default characters. Temporary for now.
 #define CMD_STATUS_REPORT '?'
 #define CMD_FEED_HOLD '!'
 #define CMD_CYCLE_START '~'
 #define CMD_RESET 0x18 // ctrl-x
 // This parameter sets the delay time before disabling the steppers after the final block of movement.
 // A short delay ensures the steppers come to a complete stop and the residual inertial force in the 
 // CNC axes don't cause the axes to drift off position. This is particularly important when manually 
@ -94,33 +102,11 @@
 // computational efficiency of generating arcs.
 #define N_ARC_CORRECTION 25 // Integer (1-255)
 // Time delay increments performed during a dwell. The default value is set at 50ms, which provides
 // a maximum time delay of roughly 55 minutes, more than enough for most any application. Increasing
 // this delay will increase the maximum dwell time linearly, but also reduces the responsiveness of 
 // run-time command executions, like status reports, since these are performed between each dwell 
 // time step. Also, keep in mind that the Arduino delay timer is not very accurate for long delays.
 #define DWELL_TIME_STEP 50 // Integer (milliseconds)
 #endif
 // Pin-assignments from Grbl 0.5
 // #define STEPPERS_DISABLE_DDR     DDRD
 // #define STEPPERS_DISABLE_PORT    PORTD
 // #define STEPPERS_DISABLE_BIT         2
 // 
 // #define STEPPING_DDR       DDRC
 // #define STEPPING_PORT      PORTC 
 // #define X_STEP_BIT           0
 // #define Y_STEP_BIT           1
 // #define Z_STEP_BIT           2
 // #define X_DIRECTION_BIT            3
 // #define Y_DIRECTION_BIT            4
 // #define Z_DIRECTION_BIT            5
 // 
 // #define LIMIT_DDR      DDRD
 // #define LIMIT_PORT     PORTD
 // #define X_LIMIT_BIT          3
 // #define Y_LIMIT_BIT          4
 // #define Z_LIMIT_BIT          5
 // 
 // #define SPINDLE_ENABLE_DDR DDRD
 // #define SPINDLE_ENABLE_PORT PORTD
 // #define SPINDLE_ENABLE_BIT 6
 // 
 // #define SPINDLE_DIRECTION_DDR DDRD
 // #define SPINDLE_DIRECTION_PORT PORTD
 // #define SPINDLE_DIRECTION_BIT 7
--- a/doc/commands.txt
+++ b/doc/commands.txt
@ -0,0 +1,21 @@
 Runtime commands for Grbl
 =========================
 In normal operation, grbl accepts g-code blocks followed by a carriage return. Each block is then parsed, processed, and placed into a ring buffer with computed acceleration profiles. Grbl will respond with an 'ok' or 'error:XXX' for each block received. 
 As of v0.8, grbl features multi-tasking events, which allow for immediate execution of run-time commands regardless of what grbl is doing. With this functionality, direct control of grbl may be possible, such as a controlled decelerating feed hold, resume, and system abort/reset. In addition, this provides the ability to report the real-time status of the CNC machine's current location and feed rates.
 How it works: The run-time commands are defined as special characters, which are picked off the serial read buffer at an interrupt level. The serial interrupt then sets a run-time command system flag for the main program to execute when ready. The main program consists of run-time command check points placed strategically in various points in the program, where grbl maybe idle waiting for room in a buffer or the execution time from the last check point may exceed a fraction of a second. 
 How to interface: From a terminal connection, run-time commands may be sent at anytime via keystrokes. When streaming g-code, the user interface should be able to send these special characters independently of the stream. Grbl will not write these run-time command special characters to the buffer, so they may be placed anywhere in the stream at anytime, as long as there is room in the buffer. Also, ensure that the g-code program being streamed to grbl does not contain any of these special characters in the program. These characters may be defined per user requirements in the 'config.h' file.
 Run-time commands:
 - Feed Hold: This initiates an immediate controlled deceleration of the streaming g-code program to a stop. The deceleration, limited by the machine acceleration settings, ensures no steps are lost and positioning is maintained. Grbl may still receive and buffer g-code blocks as  the feed hold is being executed. Once the feed hold completes, grbl will replan the buffer and resume upon a 'cycle start' command.
 - Cycle Start: (a.k.a. Resume) For now, cycle start only resumes the g-code program after a feed hold. In later releases, this may also function as a way to initiate the steppers manually when a user would like to fill the planner buffer completely before starting the cycle.
 - Reset: This issues an immediate shutdown of the stepper motors and a system abort. The main program will exit back to the main loop and re-initialize grbl.
 - Status Report: (TODO) In future releases, this will provide real-time positioning, feed rate, and block processed data, as well as other important data to the user. This also may be considered a 'poor-man's' DRO (digital read-out), where grbl thinks it is, rather than a direct and absolute measurement.
--- a/doc/planner-maths.txt
+++ b/doc/planner-maths.txt
@ -1,30 +0,0 @@
 Reasoning behind the mathematics in 'planner' module (in the key of 'Mathematica')
 ==================================================================================
 s == speed, a == acceleration, t == time, d == distance
 Basic definitions:
  Speed[s_, a_, t_] := s + (a*t) 
  Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
 Distance to reach a specific speed with a constant acceleration:
  Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
    d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
 Speed after a given distance of travel with constant acceleration:
  Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
    m -> Sqrt[2 a d + s^2]    
  DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
 When to start braking (di) to reach a specified destionation speed (s2) after accelerating
 from initial speed s1 without ever stopping at a plateau:
  Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
    di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
  IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
--- a/doc/structure.txt
+++ b/doc/structure.txt
@ -4,7 +4,8 @@ The general structure of Grbl
 The main processing stack:
 'protocol'        : Accepts command lines from the serial port and passes them to 'gcode' for execution.
-                    Provides status responses for each command.
+                    Provides status responses for each command. Also manages run-time commands set by
                    the serial interrupt.
 'gcode'           : Recieves gcode from 'protocol', parses it according to the current state
                    of the parser and issues commands via '..._control' modules
@ -14,8 +15,8 @@ The main processing stack:
 'motion_control'  : Accepts motion commands from 'gcode' and passes them to the 'planner'. This module
                    represents the public interface of the planner/stepper duo.
-'planner'         : Recieves linear motion commands from 'motion_control' and adds them to the plan of 
+'planner'         : Receives linear motion commands from 'motion_control' and adds them to the plan of 
-                    prepared motions. It takes care of continously optimizing the acceleration profile
+                    prepared motions. It takes care of continuously optimizing the acceleration profile
                    as motions are added.
 'stepper'         : Executes the motions by stepping the steppers according to the plan.
@ -27,15 +28,16 @@ Supporting files:
 'config.h'        : Compile time user settings
-'settings'        : Maintains the run time settings record in eeprom and makes it availible
+'settings'        : Maintains the run time settings record in eeprom and makes it available
                    to all modules.
 'eeprom'          : A library from Atmel that provides methods for reading and writing the eeprom with 
                    a small addition from us that read and write binary streams with check sums used 
                    to verify validity of the settings record.
-'nuts_bolts.h'    : A tiny collection of useful constants and macros used everywhere
+'nuts_bolts.h'    : A collection of global variable definitions, useful constants, and macros used everywhere
-'serial'          : Low level serial communications
+'serial'          : Low level serial communications and picks off run-time commands real-time for asynchronous 
                    control.
 'print'           : Functions to print strings of different formats (using serial)
--- a/gcode.c
+++ b/gcode.c
@ -127,10 +127,8 @@ uint8_t gc_execute_line(char *line) {
      switch(int_value) {
        case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
        case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
 #ifdef __AVR_ATmega328P__        
        case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
        case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
 #endif        
        case 4: next_action = NEXT_ACTION_DWELL; break;
        case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
        case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
@ -226,7 +224,6 @@ uint8_t gc_execute_line(char *line) {
      mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], 
        (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
      break;
 #ifdef __AVR_ATmega328P__
      case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
      if (radius_mode) {
        /* 
@ -336,7 +333,6 @@ uint8_t gc_execute_line(char *line) {
        r, isclockwise);
      break;
 #endif      
    }    
  }
--- a/limits.c
+++ b/limits.c
@ -24,6 +24,10 @@
 #include "settings.h"
 #include "nuts_bolts.h"
 #include "config.h"
 #include "motion_control.h"
 #include "planner.h"
 // TODO: Deprecated. Need to update for new version.
 void limits_init() {
  LIMIT_DDR &= ~(LIMIT_MASK);
@ -87,7 +91,7 @@ static void leave_limit_switch(bool x, bool y, bool z) {
 }
 void limits_go_home() {
-  st_synchronize();
+  plan_synchronize();
  // Store the current limit switch state
  uint8_t original_limit_state = LIMIT_PIN;
  approach_limit_switch(false, false, true); // First home the z axis
--- a/main.c
+++ b/main.c
@ -3,7 +3,9 @@
  Part of Grbl
  Copyright (c) 2009-2011 Simen Svale Skogsrud
-
+  Copyright (c) 2011 Sungeun K. Jeon
  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
@ -21,9 +23,11 @@
 #include <avr/io.h>
 #include <avr/sleep.h>
 #include <avr/interrupt.h>
 #include <avr/pgmspace.h>
 #include <util/delay.h>
 #include "config.h"
 #include "planner.h"
 #include "nuts_bolts.h"
 #include "stepper.h"
 #include "spindle_control.h"
 #include "motion_control.h"
@ -34,22 +38,56 @@
 #include "settings.h"
 #include "serial.h"
 #include "print.h"
 // Declare system global variables
 uint8_t sys_abort; // Global system abort flag
 volatile uint8_t sys_state; // Global system state variable
 int main(void)
 {
-  sei();
+  // Initialize system
-  
+  sei(); // Enable interrupts
-  serial_init(BAUD_RATE);
+  serial_init(BAUD_RATE); // Setup serial baud rate and interrupts
-  protocol_init();        
+  st_init(); // Setup stepper pins and interrupt timers
-  settings_init();  
+  sys_abort = true;   // Set abort to complete initialization
  plan_init();      
  st_init();        
  spindle_init();   
  gc_init();
  limits_init();
-  for(;;){
+  while(1) {
-    sleep_mode(); // Wait for it ...
+  
    // Upon a system abort, the main program will return to this loop. Once here, it is safe to 
    // re-initialize the system. Upon startup, the system will automatically reset to finish the 
    // initialization process.
    if (sys_abort) {
      // Execute system reset
      sys_state = 0; // Reset system state
      sys_abort = false; // Release system abort
      // Reset system.
      serial_reset_read_buffer(); // Clear serial read buffer
      settings_init(); // Load grbl settings from EEPROM
      protocol_init(); // Clear incoming line data
      plan_init(); // Clear block buffer and planner variables
      gc_init(); // Set g-code parser to default state
      spindle_init();   
      limits_init();
      // TODO: For now, the stepper subsystem tracks the absolute stepper position from the point
      // of power up or hard reset. This reset is a soft reset, where the information of the current
      // position is not lost after a system abort. This is not guaranteed to be correct, since 
      // during an abort, the steppers can lose steps in the immediate stop. However, if a feed
      // hold is performed before a system abort, this position should be correct. In the next few
      // updates, this soft reset feature will be fleshed out along with the status reporting and
      // jogging features.
      st_reset(); // Clear stepper subsystem variables. Machine position variable is not reset.
      // Print grbl initialization message
      printPgmString(PSTR("\r\nGrbl " GRBL_VERSION));
      printPgmString(PSTR("\r\n'$' to dump current settings\r\n"));
    }
    protocol_execute_runtime();
    protocol_process(); // ... process the serial protocol
  }
  return 0;   /* never reached */
 }
--- a/motion_control.c
+++ b/motion_control.c
@ -4,6 +4,7 @@
  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011 Sungeun K. Jeon
  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
@ -29,28 +30,55 @@
 #include "nuts_bolts.h"
 #include "stepper.h"
 #include "planner.h"
 #include "limits.h"
 #include "protocol.h"
-// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application.
+#include "print.h"
-void mc_dwell(double seconds) 
+
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
 // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
 // (1 minute)/feed_rate time.
 // NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line 
 // segments, must pass through this routine before being passed to the planner. The seperation of
 // mc_line and plan_buffer_line is done primarily to make backlash compensation integration simple
 // and direct.
 // TODO: Check for a better way to avoid having to push the arguments twice for non-backlash cases.
 // However, this keeps the memory requirements lower since it doesn't have to call and hold two 
 // plan_buffer_lines in memory. Grbl only has to retain the original line input variables during a
 // backlash segment(s).
 void mc_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate)
 {
-   uint16_t i = floor(seconds);
+  // TODO: Backlash compensation may be installed here. Only need direction info to track when
-   st_synchronize();
+  // to insert a backlash line motion(s) before the intended line motion. Requires its own
-   _delay_ms(floor(1000*(seconds-i))); // Delay millisecond remainder
+  // plan_check_full_buffer() and check for system abort loop.
-   while (i > 0) {
+
-     _delay_ms(1000); // Delay one second
+  // If the buffer is full: good! That means we are well ahead of the robot. 
-     i--;
+  // Remain in this loop until there is room in the buffer.
-   }
+  do {
    protocol_execute_runtime(); // Check for any run-time commands
    if (sys_abort) { return; } // Bail, if system abort.
  } while ( plan_check_full_buffer() );  
  plan_buffer_line(x, y, z, feed_rate, invert_feed_rate);
  // Auto-cycle start. 
  // TODO: Determine a more efficient and robust way of implementing the auto-starting the cycle. 
  // For example, only auto-starting when the buffer is full; if there was only one g-code command
  // sent during manual operation; or if there is buffer starvation, making sure it minimizes any
  // dwelling/motion hiccups. Additionally, these situations must not auto-start during a feed hold.
  // Only the cycle start runtime command should be able to restart the cycle after a feed hold.
  st_cycle_start();
 }
-#ifdef __AVR_ATmega328P__
+
 // Execute an arc in offset mode format. position == current xyz, target == target xyz, 
 // offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
 // the direction of helical travel, radius == circle radius, isclockwise boolean. Used
 // for vector transformation direction.
 // The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
 // segment is configured in settings.mm_per_arc_segment.  
 void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, 
  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise)
 {      
 //   int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
 //   plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
  double center_axis0 = position[axis_0] + offset[axis_0];
  double center_axis1 = position[axis_1] + offset[axis_1];
  double linear_travel = target[axis_linear] - position[axis_linear];
@ -136,17 +164,35 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
    arc_target[axis_0] = center_axis0 + r_axis0;
    arc_target[axis_1] = center_axis1 + r_axis1;
    arc_target[axis_linear] += linear_per_segment;
-    plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate);
+    mc_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate);
    // Bail mid-circle on system abort. Runtime command check already performed by mc_line.
    if (sys_abort) { return; }
  }
  // Ensure last segment arrives at target location.
-  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
+  mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
 //   plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
 }
 #endif
 // Execute dwell in seconds.
 void mc_dwell(double seconds) 
 {
   uint16_t i = floor(1000/DWELL_TIME_STEP*seconds);
   plan_synchronize();
   _delay_ms(floor(1000*seconds-i*DWELL_TIME_STEP)); // Delay millisecond remainder
   while (i > 0) {
     // NOTE: Check and execute runtime commands during dwell every <= DWELL_TIME_STEP milliseconds.
     protocol_execute_runtime();
     if (sys_abort) { return; }
     _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment
     i--;
   }
 }
 // TODO: Update limits and homing cycle subprograms for better integration with new features.
 void mc_go_home()
 {
-  st_go_home();
+  limits_go_home();  
  plan_set_current_position(0,0,0);
 }
--- a/motion_control.h
+++ b/motion_control.h
@ -25,24 +25,21 @@
 #include <avr/io.h>
 #include "planner.h"
-// NOTE: Although the following functions structurally belongs in this module, there is nothing to do but
+// NOTE: Although the following function structurally belongs in this module, there is nothing to do but
 // to forward the request to the planner.
 #define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z) 
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
 // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
 // (1 minute)/feed_rate time.
-#define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate) 
+void mc_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate);
 #define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z) 
 #ifdef __AVR_ATmega328P__
 // Execute an arc in offset mode format. position == current xyz, target == target xyz, 
 // offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
 // the direction of helical travel, radius == circle radius, isclockwise boolean. Used
 // for vector transformation direction.
 void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise);
 #endif
 // Dwell for a specific number of seconds
 void mc_dwell(double seconds);
--- a/nuts_bolts.c
+++ b/nuts_bolts.c
@ -1,3 +1,23 @@
 /*
  nuts_bolts.c - Shared functions
  Part of Grbl
  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 "nuts_bolts.h"
 #include <stdint.h>
 #include <stdlib.h>
--- a/nuts_bolts.h
+++ b/nuts_bolts.h
@ -1,5 +1,5 @@
 /*
-  motion_control.h - cartesian robot controller.
+  nuts_bolts.h - Header file for shared definitions, variables, and functions
  Part of Grbl
  Copyright (c) 2009-2011 Simen Svale Skogsrud
@ -37,6 +37,23 @@
 #define max(a,b) (((a) > (b)) ? (a) : (b))
 #define min(a,b) (((a) < (b)) ? (a) : (b))
 // Define system state bit map. Used internally by runtime protocol as runtime command flags.
 // NOTE: The system state is an unsigned 8-bit volatile variable and has a 8 flag limit. The default
 // flags are always false, so the runtime protocol only needs to check for a non-zero state value to 
 // know when there is a runtime command to execute.
 #define BIT_STATUS_REPORT    1 // bit 00000001
 #define BIT_CYCLE_START      2 // bit 00000010
 #define BIT_FEED_HOLD        4 // bit 00000100
 #define BIT_RESET            8 // bit 00001000
 #define BIT_REPLAN_CYCLE    16 // bit 00010000
 // #define                     32 // bit 00100000
 // #define                     64 // bit 01000000
 // #define                    128 // bit 10000000
 // Define global system variables
 extern uint8_t sys_abort; // Global system abort flag
 extern volatile uint8_t sys_state; // Global system state variable
 // Read a floating point value from a string. Line points to the input buffer, char_counter 
 // is the indexer pointing to the current character of the line, while double_ptr is 
 // a pointer to the result variable. Returns true when it succeeds
--- a/planner.c
+++ b/planner.c
@ -4,6 +4,7 @@
  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011 Sungeun K. Jeon
  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
@ -30,28 +31,26 @@
 #include "stepper.h"
 #include "settings.h"
 #include "config.h"
 #include "protocol.h"
 // The number of linear motions that can be in the plan at any give time
-#ifdef __AVR_ATmega328P__
+#define BLOCK_BUFFER_SIZE 20
 #define BLOCK_BUFFER_SIZE 18
 #else
 #define BLOCK_BUFFER_SIZE 5
 #endif
 static 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 next_buffer_head;                 // Index of the next buffer head
-static int32_t position[3];             // The current position of the tool in absolute steps
+static int32_t position[3];             // The planner position of the tool in absolute steps
 // static int32_t coord_offset[3];         // Current coordinate offset from machine zero in absolute steps
 static double previous_unit_vec[3];     // Unit vector of previous path line segment
 static double previous_nominal_speed;   // Nominal speed of previous path line segment
 static uint8_t acceleration_manager_enabled;   // Acceleration management active?
 // Returns the index of the next block in the ring buffer
 // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
-static int8_t next_block_index(int8_t block_index) {
+static uint8_t next_block_index(uint8_t block_index) 
 {
  block_index++;
  if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
  return(block_index);
@ -59,7 +58,8 @@ static int8_t next_block_index(int8_t block_index) {
 // Returns the index of the previous block in the ring buffer
-static int8_t prev_block_index(int8_t block_index) {
+static uint8_t prev_block_index(uint8_t block_index) 
 {
  if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
  block_index--;
  return(block_index);
@ -68,7 +68,8 @@ static int8_t prev_block_index(int8_t block_index) {
 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
 // given acceleration:
-static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) {
+static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) 
 {
  return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) );
 }
@ -86,7 +87,8 @@ static double estimate_acceleration_distance(double initial_rate, double target_
 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
 // a total travel of distance. This can be used to compute the intersection point between acceleration and
 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
-static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) {
+static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) 
 {
  return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) );
 }
@ -96,13 +98,15 @@ static double intersection_distance(double initial_rate, double final_rate, doub
 // NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed
 // in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed
 // BLOCK_BUFFER_SIZE calls per planner cycle.
-static double max_allowable_speed(double acceleration, double target_velocity, double distance) {
+static double max_allowable_speed(double acceleration, double target_velocity, double distance) 
 {
  return( sqrt(target_velocity*target_velocity-2*acceleration*distance) );
 }
 // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
-static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
+static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) 
 {
  if (!current) { return; }  // Cannot operate on nothing.
  if (next) { 
@ -128,8 +132,9 @@ static void planner_reverse_pass_kernel(block_t *previous, block_t *current, blo
 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
 // implements the reverse pass.
-static void planner_reverse_pass() {
+static void planner_reverse_pass() 
-  auto int8_t block_index = block_buffer_head;
+{
  uint8_t block_index = block_buffer_head;
  block_t *block[3] = {NULL, NULL, NULL};
  while(block_index != block_buffer_tail) {    
    block_index = prev_block_index( block_index );
@ -143,7 +148,8 @@ static void planner_reverse_pass() {
 // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
-static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
+static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) 
 {
  if(!previous) { return; }  // Begin planning after buffer_tail
  // If the previous block is an acceleration block, but it is not long enough to complete the
@ -167,8 +173,9 @@ static void planner_forward_pass_kernel(block_t *previous, block_t *current, blo
 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
 // implements the forward pass.
-static void planner_forward_pass() {
+static void planner_forward_pass() 
-  int8_t block_index = block_buffer_tail;
+{
  uint8_t block_index = block_buffer_tail;
  block_t *block[3] = {NULL, NULL, NULL};
  while(block_index != block_buffer_head) {
@ -194,8 +201,8 @@ static void planner_forward_pass() {
 // The factors represent a factor of braking and must be in the range 0.0-1.0.
 // This converts the planner parameters to the data required by the stepper controller.
 // NOTE: Final rates must be computed in terms of their respective blocks.
-static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) {
+static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) 
-  
+{  
  block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
  block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
  int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2)
@ -235,8 +242,9 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d
 // planner_recalculate() after updating the blocks. Any recalulate flagged junction will
 // compute the two adjacent trapezoids to the junction, since the junction speed corresponds 
 // to exit speed and entry speed of one another.
-static void planner_recalculate_trapezoids() {
+static void planner_recalculate_trapezoids() 
-  int8_t block_index = block_buffer_tail;
+{
  uint8_t block_index = block_buffer_tail;
  block_t *current;
  block_t *next = NULL;
@ -281,63 +289,71 @@ static void planner_recalculate_trapezoids() {
 // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round-
 // off errors. Only when planned values are converted to stepper rate parameters, these are integers.
-static void planner_recalculate() {     
+static void planner_recalculate() 
 {     
  planner_reverse_pass();
  planner_forward_pass();
  planner_recalculate_trapezoids();
 }
-void plan_init() {
+void plan_reset_buffer() 
-  block_buffer_head = 0;
+{
-  block_buffer_tail = 0;
+  block_buffer_tail = block_buffer_head;
-  plan_set_acceleration_manager_enabled(true);
+  next_buffer_head = next_block_index(block_buffer_head);
 }
 void plan_init() 
 {
  plan_reset_buffer();
  clear_vector(position);
 //  clear_vector(coord_offset);
  clear_vector_double(previous_unit_vec);
  previous_nominal_speed = 0.0;
 }
-void plan_set_acceleration_manager_enabled(uint8_t enabled) {
+void plan_discard_current_block() 
-  if ((!!acceleration_manager_enabled) != (!!enabled)) {
+{
    st_synchronize();
    acceleration_manager_enabled = !!enabled;
  }
 }
 int plan_is_acceleration_manager_enabled() {
  return(acceleration_manager_enabled);
 }
 void plan_discard_current_block() {
  if (block_buffer_head != block_buffer_tail) {
    block_buffer_tail = next_block_index( block_buffer_tail );
  }
 }
-block_t *plan_get_current_block() {
+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.
 void plan_synchronize()
 {
  while(plan_get_current_block()) { 
    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. x, y and z is the signed, absolute target position in 
-// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
+// 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(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) {
+// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
-  
+void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) 
 {
  // Prepare to set up new block
  block_t *block = &block_buffer[block_buffer_head];
  // Calculate target position in absolute steps
  int32_t target[3];
  target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
  target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
  target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);     
  // Calculate the buffer head after we push this byte
  int next_buffer_head = next_block_index( block_buffer_head );	
  // If the buffer is full: good! That means we are well ahead of the robot. 
  // Rest here until there is room in the buffer.
  while(block_buffer_tail == next_buffer_head) { sleep_mode(); }
  // Prepare to set up new block
  block_t *block = &block_buffer[block_buffer_head];
  // Compute direction bits for this block
  block->direction_bits = 0;
@ -384,92 +400,113 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
  block->rate_delta = ceil( block->step_event_count*inverse_millimeters *  
        settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick)
-  // Perform planner-enabled calculations
+  // Compute path unit vector                            
-  if (acceleration_manager_enabled) {  
+  double unit_vec[3];
    // Compute path unit vector                            
    double unit_vec[3];
-    unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
+  unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
-    unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
+  unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
-    unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;  
+  unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;  
    // 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.
    double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
-    // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
+  // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
-    if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
+  // Let a circle be tangent to both previous and current path line segments, where the junction 
-      // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
+  // deviation is defined as the distance from the junction to the closest edge of the circle, 
-      // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
+  // colinear with the circle center. The circular segment joining the two paths represents the 
-      double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
+  // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
-                         - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
+  // radius of the circle, defined indirectly by junction deviation. This may be also viewed as 
-                         - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
+  // 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
-      // Skip and use default max junction speed for 0 degree acute junction.
+  // nonlinearities of both the junction angle and junction velocity.
-      if (cos_theta < 0.95) {
+  double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
-        vmax_junction = min(previous_nominal_speed,block->nominal_speed);
+
-        // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
+  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
-        if (cos_theta > -0.95) {
+  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
-          // Compute maximum junction velocity based on maximum acceleration and junction deviation
+    // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
-          double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
+    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
-          vmax_junction = min(vmax_junction,
+    double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
-            sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
+                       - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
-        }
+                       - 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) {
      vmax_junction = min(previous_nominal_speed,block->nominal_speed);
      // 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
        double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
        vmax_junction = min(vmax_junction,
          sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
      }
    }
    block->max_entry_speed = vmax_junction;
    // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
    double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
    block->entry_speed = min(vmax_junction, v_allowable);
    // Initialize planner efficiency flags
    // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
    // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
    // the current block and next block junction speeds are guaranteed to always be at their maximum
    // junction speeds in deceleration and acceleration, respectively. This is due to how the current
    // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
    // the reverse and forward planners, the corresponding block junction speed will always be at the
    // the maximum junction speed and may always be ignored for any speed reduction checks.
    if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
    else { block->nominal_length_flag = false; }
    block->recalculate_flag = true; // Always calculate trapezoid for new block
    // Update previous path unit_vector and nominal speed
    memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
    previous_nominal_speed = block->nominal_speed;
  } else {
    // Acceleration planner disabled. Set minimum that is required.
    block->initial_rate = block->nominal_rate;
    block->final_rate = block->nominal_rate;
    block->accelerate_until = 0;
    block->decelerate_after = block->step_event_count;
    block->rate_delta = 0;
  }
  block->max_entry_speed = vmax_junction;
  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
  block->entry_speed = min(vmax_junction, v_allowable);
  // Initialize planner efficiency flags
  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
  // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
  // the current block and next block junction speeds are guaranteed to always be at their maximum
  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
  // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
  // the reverse and forward planners, the corresponding block junction speed will always be at the
  // the maximum junction speed and may always be ignored for any speed reduction checks.
  if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
  else { block->nominal_length_flag = false; }
  block->recalculate_flag = true; // Always calculate trapezoid for new block
  // Update previous path unit_vector and nominal speed
  memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
  previous_nominal_speed = block->nominal_speed;
  // Update buffer head and next buffer head indices
  block_buffer_head = next_buffer_head;  
  next_buffer_head = next_block_index(block_buffer_head);
  // Move buffer head
  block_buffer_head = next_buffer_head;     
  // Update position
  memcpy(position, target, sizeof(target)); // position[] = target[]
-  if (acceleration_manager_enabled) { planner_recalculate(); }  
+  planner_recalculate(); 
  st_cycle_start();
 }
 // Reset the planner position vector and planner speed
-void plan_set_current_position(double x, double y, double z) {
+void plan_set_current_position(double x, double y, double z) 
 {
  // Track the position offset from the initial position
  // TODO: Need to make sure coord_offset is robust and/or needed. Can be used for a soft reset,
  // where the machine position is retained after a system abort/reset. However, this is not
  // correlated to the actual machine position after a soft reset and may not be needed. This could
  // be left to a user interface to maintain.
 //   coord_offset[X_AXIS] += position[X_AXIS]; 
 //   coord_offset[Y_AXIS] += position[Y_AXIS];
 //   coord_offset[Z_AXIS] += position[Z_AXIS];  
  position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
  position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
-  position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);    
+  position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); 
 //   coord_offset[X_AXIS] -= position[X_AXIS];
 //   coord_offset[Y_AXIS] -= position[Y_AXIS];
 //   coord_offset[Z_AXIS] -= position[Z_AXIS];  
  previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
  clear_vector_double(previous_unit_vec);
 }
 // 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 flags.
  block->entry_speed = 0.0;
  block->max_entry_speed = 0.0;
  block->nominal_length_flag = false;
  block->recalculate_flag = true;
  planner_recalculate();  
 }
--- a/planner.h
+++ b/planner.h
@ -27,12 +27,12 @@
 // 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
  uint32_t steps_x, steps_y, steps_z; // Step count along each axis
  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  step_event_count;          // The number of step events required to complete this block
-  uint32_t nominal_rate;              // The nominal step rate for this block in step_events/minute
+
  // Fields used by the motion planner to manage acceleration
  double nominal_speed;               // The nominal speed for this block in mm/min  
  double entry_speed;                 // Entry speed at previous-current junction in mm/min
@ -42,12 +42,13 @@ typedef struct {
  uint8_t nominal_length_flag;        // Planner flag for nominal speed always reached
  // Settings for the trapezoid generator
-  uint32_t initial_rate;              // The jerk-adjusted step rate at start of block  
+  uint32_t initial_rate;              // The step rate at start of block  
-  uint32_t final_rate;                // The minimal rate at exit
+  uint32_t final_rate;                // The step rate at end of block
  int32_t rate_delta;                 // The steps/minute to add or subtract when changing speed (must be positive)
  uint32_t accelerate_until;          // The index of the step event on which to stop acceleration
  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
 } block_t;
 // Initialize the motion plan subsystem      
@ -65,13 +66,19 @@ void plan_discard_current_block();
 // Gets the current block. Returns NULL if buffer empty
 block_t *plan_get_current_block();
 // Enables or disables acceleration-management for upcoming blocks
 void plan_set_acceleration_manager_enabled(uint8_t enabled);
 // Is acceleration-management currently enabled?
 int plan_is_acceleration_manager_enabled();
 // Reset the position vector
-void plan_set_current_position(double x, double y, double z); 
+void plan_set_current_position(double x, double y, double z);
 // Reinitialize plan with a partially completed block
 void plan_cycle_reinitialize(int32_t step_events_remaining);
 // Reset buffer
 void plan_reset_buffer();
 // 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
--- a/protocol.c
+++ b/protocol.c
@ -29,12 +29,17 @@
 #include <math.h>
 #include "nuts_bolts.h"
 #include <avr/pgmspace.h>
 #include "stepper.h"
 #include "planner.h"
 #define LINE_BUFFER_SIZE 50
-static char line[LINE_BUFFER_SIZE];
+static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated.
-static uint8_t char_counter;
+static uint8_t char_counter; // Last character counter in line variable.
 static uint8_t iscomment; // Comment/block delete flag for processor to ignore comment characters.
-static void status_message(int status_code) {
+static void status_message(int status_code) 
 {
  if (status_code == 0) {
    printPgmString(PSTR("ok\r\n"));
  } else {
@ -55,30 +60,115 @@ static void status_message(int status_code) {
  }
 }
-void protocol_init() 
+
 void protocol_status_report()
 {
-  printPgmString(PSTR("\r\nGrbl " GRBL_VERSION));
+ // TODO: Status report data is written to the user here. This function should be able to grab a 
-  printPgmString(PSTR("\r\n"));  
+ // real-time snapshot of the stepper subprogram and the actual location of the CNC machine. At a
 // minimum, status report should return real-time location information. Other important information
 // may be distance to go on block, processed block id, and feed rate. A secondary, non-critical
 // status report may include g-code state, i.e. inch mode, plane mode, absolute mode, etc. 
 //   The report generated must be as short as possible, yet still provide the user easily readable
 // information, i.e. 'x0.23 y120.4 z2.4'. This is necessary as it minimizes the computational 
 // overhead and allows grbl to keep running smoothly, especially with g-code programs with fast, 
 // short line segments and interface setups that require real-time status reports (10-20Hz).
 printString("Query Received.\r\n"); // Notify that it's working.
 }
 void protocol_init() 
 {
  char_counter = 0; // Reset line input
  iscomment = false;
 }
 // Executes run-time commands, when required. This is called from various check points in the main
 // program, primarily where there may be a while loop waiting for a buffer to clear space or any
 // point where the execution time from the last check point may be more than a fraction of a second.
 // This is a way to execute runtime commands asynchronously (aka multitasking) with grbl's g-code
 // parsing and planning functions.
 // NOTE: The sys_state variable flags are set by the serial read subprogram, except where noted.
 void protocol_execute_runtime()
 {
  if (sys_state) { // Enter only if any bit flag is enabled
    // System abort. Steppers have already been force stopped.
    if (sys_state & BIT_RESET) {
      sys_abort = true; 
      return; // Nothing else to do but exit.
    }
    // Execute and serial print status
    if (sys_state & BIT_STATUS_REPORT) { 
      protocol_status_report();
      sys_state ^= BIT_STATUS_REPORT; // Toggle off
    }
    // Initiate stepper feed hold
    if (sys_state & BIT_FEED_HOLD) {
      st_feed_hold();
      sys_state ^= BIT_FEED_HOLD; // Toggle off   
    }
    // Re-plans the buffer after a feed hold completes
    // NOTE: BIT_REPLAN_CYCLE is set by the stepper subsystem when the feed hold is complete.
    if (sys_state & BIT_REPLAN_CYCLE) {
      st_cycle_reinitialize();
      sys_state ^= BIT_REPLAN_CYCLE; // Toggle off
    }
    if (sys_state & BIT_CYCLE_START) { 
      st_cycle_start(); // Issue cycle start command to stepper subsystem
      sys_state ^= BIT_CYCLE_START; // Toggle off
    } 
  }
 }  
 // Executes one line of input according to protocol
-uint8_t protocol_execute_line(char *line) {
+uint8_t protocol_execute_line(char *line) 
 {     
  if(line[0] == '$') {
    return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module
  // } else if { 
  //
  // JOG MODE
  //
  // TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be 
  // susceptible to other runtime commands except for e-stop. The jogging function is intended to
  // be a basic toggle on/off with controlled acceleration and deceleration to prevent skipped 
  // steps. The user would supply the desired feedrate, axis to move, and direction. Toggle on would
  // start motion and toggle off would initiate a deceleration to stop. One could 'feather' the
  // motion by repeatedly toggling to slow the motion to the desired location. Location data would 
  // need to be updated real-time and supplied to the user through status queries.
  //   More controlled exact motions can be taken care of by inputting G0 or G1 commands, which are 
  // handled by the planner. It would be possible for the jog subprogram to insert blocks into the
  // block buffer without having the planner plan them. It would need to manage de/ac-celerations 
  // on its own carefully. This approach could be effective and possibly size/memory efficient.
  } else {
    return(gc_execute_line(line));    // Everything else is gcode
  }
 }
 // Process one line of incoming serial data. Remove unneeded characters and capitalize.
 void protocol_process()
 {
-  char c;
+  uint8_t c;
-  uint8_t iscomment = false;
+  while((c = serial_read()) != SERIAL_NO_DATA) {
-  while((c = serial_read()) != SERIAL_NO_DATA) 
+    if ((c == '\n') || (c == '\r')) { // End of line reached
-  {
+
-    if ((c == '\n') || (c == '\r')) { // End of block reached
+      // Runtime command check point before executing line. Prevent any furthur line executions.
      // NOTE: If there is no line, this function should quickly return to the main program when
      // the buffer empties of non-executable data.
      protocol_execute_runtime();
      if (sys_abort) { return; } // Bail to main program upon system abort    
      if (char_counter > 0) {// Line is complete. Then execute!
-        line[char_counter] = 0; // terminate string
+        line[char_counter] = 0; // Terminate string
        status_message(protocol_execute_line(line));
      } else { 
        // Empty or comment line. Skip block.
--- a/protocol.h
+++ b/protocol.h
@ -3,6 +3,7 @@
  Part of Grbl
  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011 Sungeun K. Jeon
  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
@ -36,4 +37,7 @@ void protocol_process();
 // Executes one line of input according to protocol
 uint8_t protocol_execute_line(char *line);
 // Checks and executes a runtime command at various stop points in main program
 void protocol_execute_runtime();
 #endif
--- a/readme.textile
+++ b/readme.textile
@ -4,12 +4,33 @@ Grbl is a no-compromise, high performance, low cost alternative to parallel-port
 The controller is written in highly optimized C utilizing every clever feature of the AVR-chips to achieve precise timing and asynchronous operation. It is able to maintain more than 30kHz of stable, jitter free control pulses.
-It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported - but no support for tool offsets, functions or variables as these are apocryphal and fell into disuse after humans left G-code authoring to machines some time in the 80s.
+It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported, as well as, other basic functional g-code commands. Functions and variables are not currently supported, but may be included in future releases in a form of a pre-processor.
 Grbl includes full acceleration management with look ahead. That means the controller will look up to 20 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering.
-*Important note for Atmega 168 users:* Grbl used to be compatible with both the older Ardunios running atmega 168 and the newer with 328p. The full version of Grbl now compiles without support for circles/arcs if you target 168. If you need arcs, but not acceleration-management I am still maintaining Grbl 0.51 "in the branch called 'v0_51'":https://github.com/simen/grbl/tree/v0_51.
+*Changelog for v0.8 from v0.7:*
  - *ALPHA STATUS*: Major structural overhaul to allow for multi-tasking events and new feature sets
  - New run-time command control: Feed hold (pause), Cycle start (resume), Reset (abort), Status report (TBD)
  - Controlled feed hold with deceleration to ensure no skipped steps and loss of location.
  - After feed hold, cycle accelerations are re-planned and may be resumed.
  - System reset re-initializes grbl without resetting the Arduino.
  - Updated dwell function.
  - Restructured planner and stepper modules to become independent and ready for future features.
  - Planned features: Jog mode, status reporting, backlash compensation, improved flow control, planner optimizations
  - Reduce serial read buffer to 127 characters and increased write buffer to 31 characters (-1 ring buffer).
  - Increased planner buffer size to 20 blocks.
  - Misc bug fixes and removed deprecated acceleration enabled code.
-*Note for users upgrading from 0.51 to 0.6:* The new version has new and improved default pin-out. If nothing works when you upgrade, that is because the pulse trains are coming from the wrong pins. This is a simple matter of editing config.h – the whole legacy pin assignment is there for you to uncomment.
+*Changelog for v0.7 from v0.6:*
  - Significantly improved and optimized planner re-factoring.
  - New robust cornering algorithm, enabling smoother and faster motions.
  - Arc acceleration planning enabled by efficient vector transformation implementation.
  - Stepper subsystem re-factoring to help remove some motion issues from pre-v0.7 builds.
  - Increased dwell times.
  - G92 Coordinate offset support.
  - (Beta) Limit switch and homing cycle support.
  - Many other bug fixes and efficiency improvements.
 *Important note for Atmega 168 users:* Going forward, support for Atmega 168 will be dropped due to its limited memory and speed. However, legacy Grbl v0.51 "in the branch called 'v0_51' is still available for use.
 _The project was initially inspired by the Arduino GCode Interpreter by Mike Ellery_
--- a/serial.c
+++ b/serial.c
@ -3,6 +3,7 @@
  Part of Grbl
  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011 Sungeun K. Jeon
  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
@ -24,15 +25,13 @@
 #include <avr/interrupt.h>
 #include <avr/sleep.h>
 #include "serial.h"
 #include "config.h"
 #include "stepper.h"
 #include "nuts_bolts.h"
 #include "protocol.h"
-
+#define RX_BUFFER_SIZE 128
-#ifdef __AVR_ATmega328P__
+#define TX_BUFFER_SIZE 32
 #define RX_BUFFER_SIZE 256
 #else
 #define RX_BUFFER_SIZE 64
 #endif
 #define TX_BUFFER_SIZE 16
 uint8_t rx_buffer[RX_BUFFER_SIZE];
 uint8_t rx_buffer_head = 0;
@ -44,25 +43,25 @@ volatile uint8_t tx_buffer_tail = 0;
 static void set_baud_rate(long baud) {
  uint16_t UBRR0_value = ((F_CPU / 16 + baud / 2) / baud - 1);
-	UBRR0H = UBRR0_value >> 8;
+  UBRR0H = UBRR0_value >> 8;
-	UBRR0L = UBRR0_value;
+  UBRR0L = UBRR0_value;
 }
 void serial_init(long baud)
 {
  set_baud_rate(baud);
-	/* baud doubler off  - Only needed on Uno XXX */
+  /* baud doubler off  - Only needed on Uno XXX */
  UCSR0A &= ~(1 << U2X0);
-	// enable rx and tx
+  // enable rx and tx
  UCSR0B |= 1<<RXEN0;
  UCSR0B |= 1<<TXEN0;
-	// enable interrupt on complete reception of a byte
+  // enable interrupt on complete reception of a byte
  UCSR0B |= 1<<RXCIE0;
-	// defaults to 8-bit, no parity, 1 stop bit
+  // defaults to 8-bit, no parity, 1 stop bit
 }
 void serial_write(uint8_t data) {
@ -70,20 +69,23 @@ void serial_write(uint8_t data) {
  uint8_t next_head = tx_buffer_head + 1;
  if (next_head == TX_BUFFER_SIZE) { next_head = 0; }
-  // Wait until there's a space in the buffer
+  // Wait until there is space in the buffer
-  while (next_head == tx_buffer_tail) { sleep_mode(); };
+  while (next_head == tx_buffer_tail) { 
    protocol_execute_runtime(); // Check for any run-time commands
    if (sys_abort) { return; } // Bail, if system abort.
  }
  // Store data and advance head
  tx_buffer[tx_buffer_head] = data;
  tx_buffer_head = next_head;
  // Enable Data Register Empty Interrupt to make sure tx-streaming is running
-	UCSR0B |=  (1 << UDRIE0); 
+  UCSR0B |=  (1 << UDRIE0); 
 }
 // Data Register Empty Interrupt handler
-SIGNAL(USART_UDRE_vect) {  
+ISR(USART_UDRE_vect) {  
-  // temporary tx_buffer_tail (to optimize for volatile)
+  // Temporary tx_buffer_tail (to optimize for volatile)
  uint8_t tail = tx_buffer_tail;
  // Send a byte from the buffer	
@ -101,25 +103,45 @@ SIGNAL(USART_UDRE_vect) {
 uint8_t serial_read()
 {
-	if (rx_buffer_head == rx_buffer_tail) {
+  if (rx_buffer_head == rx_buffer_tail) {
-		return SERIAL_NO_DATA;
+    return SERIAL_NO_DATA;
-	} else {
+  } else {
-		uint8_t data = rx_buffer[rx_buffer_tail];
+    uint8_t data = rx_buffer[rx_buffer_tail];
-		rx_buffer_tail++;
+    rx_buffer_tail++;
-		if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; }
+    if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; }
-		return data;
+    return data;
-	}
+  }
 }
-SIGNAL(USART_RX_vect)
+ISR(USART_RX_vect)
 {
-	uint8_t data = UDR0;
+  uint8_t data = UDR0;
-	uint8_t next_head = rx_buffer_head + 1;
+  uint8_t next_head = rx_buffer_head + 1;
-	if (next_head == RX_BUFFER_SIZE) { next_head = 0; }
+  if (next_head == RX_BUFFER_SIZE) { next_head = 0; }
  // Write data to buffer unless it is full.
-	if (next_head != rx_buffer_tail) {
+  if (next_head != rx_buffer_tail) {
-		rx_buffer[rx_buffer_head] = data;
+    // Pick off runtime command characters directly from the serial stream. These characters are
-		rx_buffer_head = next_head;
+    // not passed into the buffer, but these set system state flag bits for runtime execution.
-	}
+    switch (data) {
      case CMD_STATUS_REPORT: sys_state |= BIT_STATUS_REPORT; break; // Set as true
      case CMD_CYCLE_START:   sys_state |= BIT_CYCLE_START; break; // Set as true
      case CMD_FEED_HOLD:     sys_state |= BIT_FEED_HOLD; break; // Set as true
      case CMD_RESET: 
        // Immediately force stepper subsystem idle at an interrupt level.
        if (!(sys_state & BIT_RESET)) { // Force stop only first time.
          st_go_idle();  
        }
        sys_state |= BIT_RESET; // Set as true
        break;
      default : // Write character to buffer
        rx_buffer[rx_buffer_head] = data;
        rx_buffer_head = next_head;
    }
  }
 }
 void serial_reset_read_buffer() 
 {
  rx_buffer_tail = rx_buffer_head;
 }  
--- a/serial.h
+++ b/serial.h
@ -3,6 +3,7 @@
  Part of Grbl
  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011 Sungeun K. Jeon
  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
@ -32,4 +33,7 @@ void serial_write(uint8_t data);
 uint8_t serial_read();
 // Reset and empty data in read buffer. Used by e-stop and reset.
 void serial_reset_read_buffer();
 #endif
--- a/settings.c
+++ b/settings.c
@ -177,9 +177,7 @@ void settings_store_setting(int parameter, double value) {
 // Initialize the config subsystem
 void settings_init() {
-  if(read_settings()) {
+  if(!read_settings()) {
    printPgmString(PSTR("'$' to dump current settings\r\n"));
  } else {
    printPgmString(PSTR("Warning: Failed to read EEPROM settings. Using defaults.\r\n"));
    settings_reset();
    write_settings();
--- a/settings.h
+++ b/settings.h
@ -26,7 +26,7 @@
 #include <math.h>
 #include <inttypes.h>
-#define GRBL_VERSION "0.7d"
+#define GRBL_VERSION "0.8a"
 // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl
 // when firmware is upgraded. Always stored in byte 0 of eeprom
--- a/spindle_control.c
+++ b/spindle_control.c
@ -20,11 +20,14 @@
 #include "spindle_control.h"
 #include "settings.h"
 #include "motion_control.h"
 #include "config.h"
-#include "stepper.h"
+#include "planner.h"
 #include <avr/io.h>
 // TODO: Deprecated. Need to update for new version.
 static int current_direction;
 void spindle_init()
@ -35,7 +38,7 @@ void spindle_init()
 void spindle_run(int direction, uint32_t rpm) 
 {
  if (direction != current_direction) {
-    st_synchronize();
+    plan_synchronize();
    if(direction) {
      if(direction > 0) {
        SPINDLE_DIRECTION_PORT &= ~(1<<SPINDLE_DIRECTION_BIT);
--- a/stepper.c
+++ b/stepper.c
@ -4,6 +4,7 @@
  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011 Sungeun K. Jeon
  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
@ -33,6 +34,8 @@
 #include "planner.h"
 #include "limits.h"
 #include "print.h"
 // Some useful constants
 #define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
 #define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
@ -41,23 +44,34 @@
 #define TICKS_PER_MICROSECOND (F_CPU/1000000)
 #define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
 // Stepper state variable. Contains running data and trapezoid variables.
 typedef struct {
  volatile uint8_t cycle_start;  // Cycle start flag
  volatile uint8_t feed_hold; // Feed hold flag
  // Used by the bresenham line algorithm
  int32_t counter_x,        // Counter variables for the bresenham line tracer
          counter_y, 
          counter_z;
  uint32_t event_count;
  uint32_t step_events_completed;  // The number of step events left in current motion
  // Used by the trapezoid generator
  uint32_t cycles_per_step_event;        // The number of machine cycles between each step event
  uint32_t trapezoid_tick_cycle_counter; // The cycles since last trapezoid_tick. Used to generate ticks at a steady
                                              // pace without allocating a separate timer
  uint32_t trapezoid_adjusted_rate;      // The current rate of step_events according to the trapezoid generator
  uint32_t min_safe_rate;  // Minimum safe rate for full deceleration rate reduction step. Otherwise halves step_rate.
 } stepper_state_t;
 static stepper_state_t st;
 static int32_t st_position[3];  // Track current position in steps from initialization state.
 static block_t *current_block;  // A pointer to the block currently being traced
-// Variables used by The Stepper Driver Interrupt
+// 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
-static int32_t counter_x,       // Counter variables for the bresenham line tracer
+static volatile uint8_t busy;   // True when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
               counter_y, 
               counter_z;       
 static uint32_t step_events_completed; // The number of step events executed in the current block
 static volatile int busy; // true when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
 // Variables used by the trapezoid generation
 static uint32_t cycles_per_step_event;        // The number of machine cycles between each step event
 static uint32_t trapezoid_tick_cycle_counter; // The cycles since last trapezoid_tick. Used to generate ticks at a steady
                                              // pace without allocating a separate timer
 static uint32_t trapezoid_adjusted_rate;      // The current rate of step_events according to the trapezoid generator
 static uint32_t min_safe_rate;  // Minimum safe rate for full deceleration rate reduction step. Otherwise halves step_rate.
 static uint8_t cycle_start;     // Cycle start flag to indicate program start and block processing.
 //         __________________________
 //        /|                        |\     _________________         ^
@ -79,9 +93,12 @@ static uint8_t cycle_start;     // Cycle start flag to indicate program start an
 static void set_step_events_per_minute(uint32_t steps_per_minute);
 // Stepper state initialization
-void st_wake_up() {
+static void st_wake_up() 
 {
  // Initialize stepper output bits
-  out_bits = (0) ^ (settings.invert_mask);
+  out_bits = (0) ^ (settings.invert_mask); 
  // Set step pulse time. Ad hoc computation from oscilloscope.
  step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
  // Enable steppers by resetting the stepper disable port
  STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
  // Enable stepper driver interrupt
@ -89,9 +106,8 @@ void st_wake_up() {
 }
 // Stepper shutdown
-static void st_go_idle() {
+void st_go_idle() 
-  // Cycle finished. Set flag to false.
+{
  cycle_start = false; 
  // Disable stepper driver interrupt
  TIMSK1 &= ~(1<<OCIE1A); 
  // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
@ -103,22 +119,14 @@ static void st_go_idle() {
  STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
 }
 // Initializes the trapezoid generator from the current block. Called whenever a new 
 // block begins.
 static void trapezoid_generator_reset() {
  trapezoid_adjusted_rate = current_block->initial_rate;
  min_safe_rate = current_block->rate_delta + (current_block->rate_delta >> 1); // 1.5 x rate_delta
  trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule.
  set_step_events_per_minute(trapezoid_adjusted_rate); // Initialize cycles_per_step_event
 }
 // This function determines an acceleration velocity change every CYCLES_PER_ACCELERATION_TICK by
 // keeping track of the number of elapsed cycles during a de/ac-celeration. The code assumes that 
 // step_events occur significantly more often than the acceleration velocity iterations.
-static uint8_t iterate_trapezoid_cycle_counter() {
+static uint8_t iterate_trapezoid_cycle_counter() 
-  trapezoid_tick_cycle_counter += cycles_per_step_event;  
+{
-  if(trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) {
+  st.trapezoid_tick_cycle_counter += st.cycles_per_step_event;  
-    trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK;
+  if(st.trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) {
    st.trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK;
    return(true);
  } else {
    return(false);
@ -129,36 +137,48 @@ static uint8_t iterate_trapezoid_cycle_counter() {
 // config_step_timer. It 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.
-SIGNAL(TIMER1_COMPA_vect)
+// NOTE: ISR_NOBLOCK allows SIG_OVERFLOW2 to trigger on-time regardless of time in this handler.
 // TODO: ISR_NOBLOCK is the same as the old SIGNAL with sei() method, but is optimizable by the compiler. On
 // an oscilloscope there is a weird hitch in the step pulse during high load operation. Very infrequent, but
 // when this does happen most of the time the pulse falling edge is randomly delayed by 20%-50% of the total 
 // intended pulse time, but sometimes it pulses less than 3usec. The former likely caused by the serial 
 // interrupt doing its thing, not that big of a deal, but the latter cause is unknown and worrisome. Need
 // to track down what is causing this problem. Functionally, this shouldn't cause any noticeable issues
 // as long as stepper drivers have a pulse minimum of 1usec or so (Pololu and any Allegro IC are ok).
 ISR(TIMER1_COMPA_vect,ISR_NOBLOCK)
 {        
-  // TODO: Check if the busy-flag can be eliminated by just disabling this interrupt while we are in it
+  if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
  busy = true;
  if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt
  // Set the direction pins a couple of nanoseconds before we step the steppers
  STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
  // Then pulse the stepping pins
  STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits;
  // Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
  // exactly settings.pulse_microseconds microseconds.
-//   TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8);
+  TCNT2 = step_pulse_time;
  TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Bit shift divide by 8.
  busy = true;
  sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
         // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered 
         // at exactly the right time even if we occasionally spend a lot of time inside this handler.))
  // If there is no current block, attempt to pop one from the buffer
  if (current_block == NULL) {
-    // Anything in the buffer?
+    // Anything in the buffer? If so, initialize next motion.
    current_block = plan_get_current_block();
    if (current_block != NULL) {
-      trapezoid_generator_reset();
+      if (!st.feed_hold) { // During feed hold, do not update rate and trap counter. Keep decelerating.
-      counter_x = -(current_block->step_event_count >> 1);
+        st.trapezoid_adjusted_rate = current_block->initial_rate;
-      counter_y = counter_x;
+        set_step_events_per_minute(st.trapezoid_adjusted_rate); // Initialize cycles_per_step_event
-      counter_z = counter_x;
+        st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule.
-      step_events_completed = 0;     
+      }
      st.min_safe_rate = current_block->rate_delta + (current_block->rate_delta >> 1); // 1.5 x rate_delta
      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_completed = 0;     
    } else {
      st.cycle_start = false;
      st.feed_hold = false;
      st_go_idle();
    }    
  } 
@ -166,139 +186,167 @@ SIGNAL(TIMER1_COMPA_vect)
  if (current_block != NULL) {
    // Execute step displacement profile by bresenham line algorithm
    out_bits = current_block->direction_bits;
-    counter_x += current_block->steps_x;
+    st.counter_x += current_block->steps_x;
-    if (counter_x > 0) {
+    if (st.counter_x > 0) {
      out_bits |= (1<<X_STEP_BIT);
-      counter_x -= current_block->step_event_count;
+      st.counter_x -= st.event_count;
      if (out_bits & (1<<X_DIRECTION_BIT)) { st_position[X_AXIS]--; }
      else { st_position[X_AXIS]++; }
    }
-    counter_y += current_block->steps_y;
+    st.counter_y += current_block->steps_y;
-    if (counter_y > 0) {
+    if (st.counter_y > 0) {
      out_bits |= (1<<Y_STEP_BIT);
-      counter_y -= current_block->step_event_count;
+      st.counter_y -= st.event_count;
      if (out_bits & (1<<Y_DIRECTION_BIT)) { st_position[Y_AXIS]--; }
      else { st_position[Y_AXIS]++; }
    }
-    counter_z += current_block->steps_z;
+    st.counter_z += current_block->steps_z;
-    if (counter_z > 0) {
+    if (st.counter_z > 0) {
      out_bits |= (1<<Z_STEP_BIT);
-      counter_z -= current_block->step_event_count;
+      st.counter_z -= st.event_count;
      if (out_bits & (1<<Z_DIRECTION_BIT)) { st_position[Z_AXIS]--; }
      else { st_position[Z_AXIS]++; }
    }
-    step_events_completed++; // Iterate step events
+    st.step_events_completed++; // Iterate step events
    // While in block steps, check for de/ac-celeration events and execute them accordingly.
-    if (step_events_completed < current_block->step_event_count) {
+    if (st.step_events_completed < current_block->step_event_count) {
-      
+      if (st.feed_hold) {
-      // The trapezoid generator always checks step event location to ensure de/ac-celerations are 
+        // Check for and execute feed hold by enforcing a steady deceleration from the moment of 
-      // executed and terminated at exactly the right time. This helps prevent over/under-shooting
+        // execution. The rate of deceleration is limited by rate_delta and will never decelerate
-      // the target position and speed. 
+        // faster or slower than in normal operation. If the distance required for the feed hold 
-      
+        // deceleration spans more than one block, the initial rate of the following blocks are not
-      // NOTE: By increasing the ACCELERATION_TICKS_PER_SECOND in config.h, the resolution of the 
+        // updated and deceleration is continued according to their corresponding rate_delta.
-      // discrete velocity changes increase and accuracy can increase as well to a point. Numerical 
+        // NOTE: The trapezoid tick cycle counter is not updated intentionally. This ensures that 
-      // round-off errors can effect this, if set too high. This is important to note if a user has 
+        // the deceleration is smooth regardless of where the feed hold is initiated and if the
-      // very high acceleration and/or feedrate requirements for their machine.
+        // deceleration distance spans multiple blocks.
-      
+        if ( iterate_trapezoid_cycle_counter() ) {                    
-      if (step_events_completed < current_block->accelerate_until) {
+          // If deceleration complete, set system flags and shutdown steppers.
-        // Iterate cycle counter and check if speeds need to be increased.
+          if (st.trapezoid_adjusted_rate <= current_block->rate_delta) {
-        if ( iterate_trapezoid_cycle_counter() ) {
+            // Just go idle. Do not NULL current block. The bresenham algorithm variables must
-          trapezoid_adjusted_rate += current_block->rate_delta;
+            // remain intact to ensure the stepper path is exactly the same.
-          if (trapezoid_adjusted_rate >= current_block->nominal_rate) {
+            st.cycle_start = false;
-            // Reached nominal rate a little early. Cruise at nominal rate until decelerate_after.
+            st_go_idle();
-            trapezoid_adjusted_rate = current_block->nominal_rate;
+            sys_state |= BIT_REPLAN_CYCLE; // Flag main program that feed hold is complete.
-          }
+          } else {
-          set_step_events_per_minute(trapezoid_adjusted_rate);
+            st.trapezoid_adjusted_rate -= current_block->rate_delta;
-        }
+            set_step_events_per_minute(st.trapezoid_adjusted_rate);
-      } else if (step_events_completed >= current_block->decelerate_after) {
+          }      
        // Reset trapezoid tick cycle counter to make sure that the deceleration is performed the
        // same every time. Reset to CYCLES_PER_ACCELERATION_TICK/2 to follow the midpoint rule for
        // an accurate approximation of the deceleration curve.
        if (step_events_completed == current_block-> decelerate_after) {
          trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2;
        } else {
          // Iterate cycle counter and check if speeds need to be reduced.
          if ( iterate_trapezoid_cycle_counter() ) {  
            // NOTE: We will only do a full speed reduction if the result is more than the minimum safe 
            // rate, initialized in trapezoid reset as 1.5 x rate_delta. Otherwise, reduce the speed by
            // half increments until finished. The half increments are guaranteed not to exceed the 
            // CNC acceleration limits, because they will never be greater than rate_delta. This catches
            // small errors that might leave steps hanging after the last trapezoid tick or a very slow
            // step rate at the end of a full stop deceleration in certain situations. The half rate 
            // reductions should only be called once or twice per block and create a nice smooth 
            // end deceleration.
            if (trapezoid_adjusted_rate > min_safe_rate) {
              trapezoid_adjusted_rate -= current_block->rate_delta;
            } else {
              trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2
            }
            if (trapezoid_adjusted_rate < current_block->final_rate) {
              // Reached final rate a little early. Cruise to end of block at final rate.
              trapezoid_adjusted_rate = current_block->final_rate;
            }
            set_step_events_per_minute(trapezoid_adjusted_rate);
          }
        }
      } else {
-        // No accelerations. Make sure we cruise exactly at the nominal rate.
+        // The trapezoid generator always checks step event location to ensure de/ac-celerations are 
-        if (trapezoid_adjusted_rate != current_block->nominal_rate) {
+        // executed and terminated at exactly the right time. This helps prevent over/under-shooting
-          trapezoid_adjusted_rate = current_block->nominal_rate;
+        // the target position and speed. 
-          set_step_events_per_minute(trapezoid_adjusted_rate);
+        // NOTE: By increasing the ACCELERATION_TICKS_PER_SECOND in config.h, the resolution of the 
        // discrete velocity changes increase and accuracy can increase as well to a point. Numerical 
        // round-off errors can effect this, if set too high. This is important to note if a user has 
        // very high acceleration and/or feedrate requirements for their machine.
        if (st.step_events_completed < current_block->accelerate_until) {
          // Iterate cycle counter and check if speeds need to be increased.
          if ( iterate_trapezoid_cycle_counter() ) {
            st.trapezoid_adjusted_rate += current_block->rate_delta;
            if (st.trapezoid_adjusted_rate >= current_block->nominal_rate) {
              // Reached nominal rate a little early. Cruise at nominal rate until decelerate_after.
              st.trapezoid_adjusted_rate = current_block->nominal_rate;
            }
            set_step_events_per_minute(st.trapezoid_adjusted_rate);
          }
        } else if (st.step_events_completed >= current_block->decelerate_after) {
          // Reset trapezoid tick cycle counter to make sure that the deceleration is performed the
          // same every time. Reset to CYCLES_PER_ACCELERATION_TICK/2 to follow the midpoint rule for
          // an accurate approximation of the deceleration curve.
          if (st.step_events_completed == current_block-> decelerate_after) {
            st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2;
          } else {
            // Iterate cycle counter and check if speeds need to be reduced.
            if ( iterate_trapezoid_cycle_counter() ) {  
              // NOTE: We will only do a full speed reduction if the result is more than the minimum safe 
              // rate, initialized in trapezoid reset as 1.5 x rate_delta. Otherwise, reduce the speed by
              // half increments until finished. The half increments are guaranteed not to exceed the 
              // CNC acceleration limits, because they will never be greater than rate_delta. This catches
              // small errors that might leave steps hanging after the last trapezoid tick or a very slow
              // step rate at the end of a full stop deceleration in certain situations. The half rate 
              // reductions should only be called once or twice per block and create a nice smooth 
              // end deceleration.
              if (st.trapezoid_adjusted_rate > st.min_safe_rate) {
                st.trapezoid_adjusted_rate -= current_block->rate_delta;
              } else {
                st.trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2
              }
              if (st.trapezoid_adjusted_rate < current_block->final_rate) {
                // Reached final rate a little early. Cruise to end of block at final rate.
                st.trapezoid_adjusted_rate = current_block->final_rate;
              }
              set_step_events_per_minute(st.trapezoid_adjusted_rate);
            }
          }
        } else {
          // No accelerations. Make sure we cruise exactly at the nominal rate.
          if (st.trapezoid_adjusted_rate != current_block->nominal_rate) {
            st.trapezoid_adjusted_rate = current_block->nominal_rate;
            set_step_events_per_minute(st.trapezoid_adjusted_rate);
          }
        }
-      }
+      }            
    } else {   
      // If current block is finished, reset pointer 
      current_block = NULL;
      plan_discard_current_block();
    }
  }
  out_bits ^= settings.invert_mask;  // Apply stepper invert mask    
-  busy=false;
+  busy = false;
 }
 // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
 // the motor port after a short period (settings.pulse_microseconds) completing one step cycle.
-SIGNAL(TIMER2_OVF_vect)
+ISR(TIMER2_OVF_vect)
 {
-  // reset stepping pins (leave the direction pins)
+  // Reset stepping pins (leave the direction pins)
  STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
 }
 // Reset and clear stepper subsystem variables
 void st_reset()
 {
  memset(&st, 0, sizeof(st));
  set_step_events_per_minute(MINIMUM_STEPS_PER_MINUTE);
  current_block = NULL;
  busy = false;
 }
 // Initialize and start the stepper motor subsystem
 void st_init()
 {
-	// Configure directions of interface pins
+  // Configure directions of interface pins
-  STEPPING_DDR   |= STEPPING_MASK;
+  STEPPING_DDR |= STEPPING_MASK;
  STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
  STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
-	// waveform generation = 0100 = CTC
+  // waveform generation = 0100 = CTC
-	TCCR1B &= ~(1<<WGM13);
+  TCCR1B &= ~(1<<WGM13);
-	TCCR1B |=  (1<<WGM12);
+  TCCR1B |=  (1<<WGM12);
-	TCCR1A &= ~(1<<WGM11); 
+  TCCR1A &= ~(1<<WGM11); 
-	TCCR1A &= ~(1<<WGM10);
+  TCCR1A &= ~(1<<WGM10);
-	// output mode = 00 (disconnected)
+  // output mode = 00 (disconnected)
-	TCCR1A &= ~(3<<COM1A0); 
+  TCCR1A &= ~(3<<COM1A0); 
-	TCCR1A &= ~(3<<COM1B0); 
+  TCCR1A &= ~(3<<COM1B0); 
-	// Configure Timer 2
+  // Configure Timer 2
  TCCR2A = 0;         // Normal operation
  TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
  TIMSK2 |= (1<<TOIE2);      
-  set_step_events_per_minute(6000);
+  // Initialize machine position vector
-  trapezoid_tick_cycle_counter = 0;
+  clear_vector(st_position);
-  
+
  // Start in the idle state
  st_go_idle();
 }
 // Block until all buffered steps are executed
 void st_synchronize()
 {
  while(plan_get_current_block()) { sleep_mode(); }    
 }
 // Configures the prescaler and ceiling of timer 1 to produce the given rate as accurately as possible.
 // Returns the actual number of cycles per interrupt
 static uint32_t config_step_timer(uint32_t cycles)
@ -306,54 +354,80 @@ static uint32_t config_step_timer(uint32_t cycles)
  uint16_t ceiling;
  uint16_t prescaler;
  uint32_t actual_cycles;
-	if (cycles <= 0xffffL) {
+  if (cycles <= 0xffffL) {
-		ceiling = cycles;
+    ceiling = cycles;
    prescaler = 0; // prescaler: 0
    actual_cycles = ceiling;
-	} else if (cycles <= 0x7ffffL) {
+  } else if (cycles <= 0x7ffffL) {
    ceiling = cycles >> 3;
    prescaler = 1; // prescaler: 8
    actual_cycles = ceiling * 8L;
-	} else if (cycles <= 0x3fffffL) {
+  } else if (cycles <= 0x3fffffL) {
-		ceiling =  cycles >> 6;
+    ceiling =  cycles >> 6;
    prescaler = 2; // prescaler: 64
    actual_cycles = ceiling * 64L;
-	} else if (cycles <= 0xffffffL) {
+  } else if (cycles <= 0xffffffL) {
-		ceiling =  (cycles >> 8);
+    ceiling =  (cycles >> 8);
    prescaler = 3; // prescaler: 256
    actual_cycles = ceiling * 256L;
-	} else if (cycles <= 0x3ffffffL) {
+  } else if (cycles <= 0x3ffffffL) {
-		ceiling = (cycles >> 10);
+    ceiling = (cycles >> 10);
    prescaler = 4; // prescaler: 1024
    actual_cycles = ceiling * 1024L;    
-	} else {
+  } else {
-	  // Okay, that was slower than we actually go. Just set the slowest speed
+    // Okay, that was slower than we actually go. Just set the slowest speed
-		ceiling = 0xffff;
+    ceiling = 0xffff;
    prescaler = 4;
    actual_cycles = 0xffff * 1024;
-	}
+  }
-	// Set prescaler
+  // Set prescaler
  TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
  // Set ceiling
  OCR1A = ceiling;
  return(actual_cycles);
 }
-static void set_step_events_per_minute(uint32_t steps_per_minute) {
+static void set_step_events_per_minute(uint32_t steps_per_minute) 
  if (steps_per_minute < MINIMUM_STEPS_PER_MINUTE) { steps_per_minute = MINIMUM_STEPS_PER_MINUTE; }
  cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute);
 }
 void st_go_home()
 {
-  limits_go_home();  
+  if (steps_per_minute < MINIMUM_STEPS_PER_MINUTE) { steps_per_minute = MINIMUM_STEPS_PER_MINUTE; }
-  plan_set_current_position(0,0,0);
+  st.cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute);
 }
-// Planner external interface to start stepper interrupt and execute the blocks in queue.
+// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
-void st_cycle_start() {
+// by planner auto-start and run-time command functions.
-  if (!cycle_start) {
+void st_cycle_start() 
-    cycle_start = true;
+{
-    st_wake_up();
+  if (!st.cycle_start) {
    if (!st.feed_hold) {
      st.cycle_start = true;
      st_wake_up();
    }
  }
 }
 // Execute a feed hold with deceleration, only during cycle. Called by main program.
 void st_feed_hold() 
 {
  if (!st.feed_hold) {
    if (st.cycle_start) {
      st.feed_hold = true;
    }
  }
 }
 // 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()
 {
  // Replan buffer from the feed hold stop location.
  plan_cycle_reinitialize(current_block->step_event_count - st.step_events_completed);
  // Update initial rate and timers after feed hold.
  st.trapezoid_adjusted_rate = 0; // Resumes from rest
  set_step_events_per_minute(st.trapezoid_adjusted_rate);
  st.trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule.
  st.step_events_completed = 0;
  st.feed_hold = false; // Release feed hold. Cycle is ready to re-start.
 }
--- a/stepper.h
+++ b/stepper.h
@ -3,6 +3,7 @@
  Part of Grbl
  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011 Sungeun K. Jeon
  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
@ -29,16 +30,22 @@
 #define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
 #define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
-// Initialize and start the stepper motor subsystem
+// Initialize and setup the stepper motor subsystem
 void st_init();
-// Block until all buffered steps are executed
+// Immediately disables steppers
-void st_synchronize();
+void st_go_idle();
-// Execute the homing cycle
+// Reset the stepper subsystem variables       
-void st_go_home();
+void st_reset();
 // Notify the stepper subsystem to start executing the g-code program in buffer.
 void st_cycle_start();
 // Reinitializes the buffer after a feed hold for a resume.
 void st_cycle_reinitialize(); 
 // Initiates a feed hold of the running program
 void st_feed_hold();
 #endif