diff --git a/config.h b/config.h
index b164648..a43f15c 100644
--- a/config.h
+++ b/config.h
@@ -122,7 +122,7 @@
 // frequency goes up. So there will be little left for other processes like arcs.  
 //   In future versions, more work will be done to increase the step rates but still stay around
 // 20kHz by performing two steps per step event, rather than just one.
-#define ISR_TICKS_PER_SECOND 30000L  // Integer (Hz)
+#define ISR_TICKS_PER_SECOND 20000L  // Integer (Hz)
 
 // The Ranade algorithm can use either floating point or long integers for its counters, but for 
 // integers the counter values must be scaled since these values can be very small (10^-6). This
diff --git a/gcode.c b/gcode.c
index c741fc2..5d3404f 100644
--- a/gcode.c
+++ b/gcode.c
@@ -51,8 +51,7 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
 void gc_init() 
 {
   memset(&gc, 0, sizeof(gc));
-  gc.feed_rate = settings.default_feed_rate; // Should be zero at initialization.
-//  gc.seek_rate = settings.default_seek_rate;
+  gc.feed_rate = settings.default_feed_rate;
   select_plane(X_AXIS, Y_AXIS, Z_AXIS);
   gc.absolute_mode = true;
   
@@ -247,9 +246,7 @@ uint8_t gc_execute_line(char *line)
      NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency 
      and simplicity purposes, but this should not affect proper g-code execution. */
   
-  // ([F]: Set feed and seek rates.)
-  // TODO: Seek rates can change depending on the direction and maximum speeds of each axes. When
-  // max axis speed is installed, the calculation can be performed here, or maybe in the planner.
+  // ([F]: Set feed rate.)
     
   if (sys.state != STATE_CHECK_MODE) { 
     //  ([M6]: Tool change should be executed here.)
@@ -324,14 +321,14 @@ uint8_t gc_execute_line(char *line)
             target[i] = gc.position[i];
           }
         }
-        mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false);
+        mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], -1.0, false);
       }
       // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
       float coord_data[N_AXIS];
       uint8_t home_select = SETTING_INDEX_G28;
       if (non_modal_action == NON_MODAL_GO_HOME_1) { home_select = SETTING_INDEX_G30; }
       if (!settings_read_coord_data(home_select,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
-      mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], settings.default_seek_rate, false); 
+      mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], -1.0, false); 
       memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
       axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
       break;
@@ -347,7 +344,7 @@ uint8_t gc_execute_line(char *line)
         // Update axes defined only in block. Offsets current system to defined value. Does not update when
         // active coordinate system is selected, but is still active unless G92.1 disables it. 
         uint8_t i;
-        for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used.
+        for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
           if (bit_istrue(axis_words,bit(i)) ) {
             gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
           }
@@ -382,7 +379,7 @@ uint8_t gc_execute_line(char *line)
     // absolute mode coordinate offsets or incremental mode offsets.
     // NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
     uint8_t i;
-    for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used to save flash space.
+    for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used to save flash space.
       if ( bit_istrue(axis_words,bit(i)) ) {
         if (!absolute_override) { // Do not update target in absolute override mode
           if (gc.absolute_mode) {
@@ -402,7 +399,7 @@ uint8_t gc_execute_line(char *line)
         break;
       case MOTION_MODE_SEEK:
         if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} 
-        else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false); }
+        else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], -1.0, false); }
         break;
       case MOTION_MODE_LINEAR:
         // TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
diff --git a/gcode.h b/gcode.h
index f3671c2..1cfd3d0 100644
--- a/gcode.h
+++ b/gcode.h
@@ -72,7 +72,6 @@ typedef struct {
   int8_t spindle_direction;        // 1 = CW, -1 = CCW, 0 = Stop {M3, M4, M5}
   uint8_t coolant_mode;            // 0 = Disable, 1 = Flood Enable {M8, M9}
   float feed_rate;                 // Millimeters/min
-//  float seek_rate;                 // Millimeters/min. Will be used in v0.9 when axis independence is installed
   float position[3];               // Where the interpreter considers the tool to be at this point in the code
   uint8_t tool;
 //  uint16_t spindle_speed;          // RPM/100
diff --git a/main.c b/main.c
index c03735f..b1e7734 100644
--- a/main.c
+++ b/main.c
@@ -103,6 +103,11 @@ int main(void)
     protocol_execute_runtime();
     protocol_process(); // ... process the serial protocol
     
+    // When the serial protocol returns, there are no more characters in the serial read buffer to
+    // be processed and executed. This indicates that individual commands are being issued or 
+    // streaming is finished. In either case, auto-cycle start, if enabled, any queued moves.
+    if (sys.auto_start) { st_cycle_start(); }
+    
   }
   return 0;   /* never reached */
 }
diff --git a/motion_control.c b/motion_control.c
index 2d595c8..2fe7872 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -41,8 +41,8 @@
 // (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.
+// mc_line and plan_buffer_line is done primarily to make backlash compensation or canned cycle
+// 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
@@ -83,7 +83,7 @@ void mc_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rat
   // when the buffer is completely full and primed; auto-starting, if there was only one g-code 
   // command sent during manual operation; or if a system is prone to buffer starvation, auto-start
   // helps make sure it minimizes any dwelling/motion hiccups and keeps the cycle going. 
-  if (sys.auto_start) { st_cycle_start(); }
+  // if (sys.auto_start) { st_cycle_start(); }
 }
 
 
diff --git a/planner.c b/planner.c
index 08359b9..7a3b303 100644
--- a/planner.c
+++ b/planner.c
@@ -31,6 +31,9 @@
 #include "config.h"
 #include "protocol.h"
 
+#define SOME_LARGE_VALUE 1000000.0 // Used by rapids and acceleration maximization calculations. Just needs
+                                   // to be larger than any feasible mm/min or mm/sec^2 value.
+
 static block_t block_buffer[BLOCK_BUFFER_SIZE];  // A ring buffer for motion instructions
 static volatile uint8_t block_buffer_head;       // Index of the next block to be pushed
 static volatile uint8_t block_buffer_tail;       // Index of the block to process now
@@ -43,10 +46,17 @@ typedef struct {
                                      // i.e. arcs, canned cycles, and backlash compensation.
   float previous_unit_vec[N_AXIS];   // Unit vector of previous path line segment
   float previous_nominal_speed_sqr;  // Nominal speed of previous path line segment
-  float inverse_steps_per_mm[N_AXIS];
+  float last_x, last_y, last_z;      // Target position of previous path line segment
 } planner_t;
 static planner_t pl;
 
+
+void plan_init() 
+{
+  plan_reset_buffer();
+  memset(&pl, 0, sizeof(pl)); // Clear planner struct
+}
+
 // Returns the index of the next block in the ring buffer
 // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
 static uint8_t next_block_index(uint8_t block_index) 
@@ -93,10 +103,9 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr,
 {  
   // Compute new initial rate for stepper algorithm
   block->initial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
-  
+          
   // TODO: Compute new nominal rate if a feedrate override occurs.
   // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
-
     
   // Compute efficiency variable for following calculations. Removes a float divide and multiply.
   // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds.
@@ -155,11 +164,29 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr,
   All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only
   when planned values are converted to stepper rate parameters(3), these are integers. If another motion block
   is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes.
+  
+  Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's
+  constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and
+  nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile
+  through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting
+  conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile.
+  
+  Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line
+  segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled 
+  in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the
+  buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be 
+  able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance.
+  The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED.
+  Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost 
+  steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the 
+  bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory 
+  is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering 
+  up to a hundred or more.
 
-  NOTE: As executing blocks complete and incoming streaming blocks are appended to the planner buffer, this
-  function is constantly re-calculating and must be as efficient as possible. For example, in situations like 
-  arc generation or complex curves, the short, rapid line segments can execute faster than new blocks can be 
-  added, and the planner buffer will then starve and empty, leading to weird hiccup-like jerky motions.
+  NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient
+  as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments
+  can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading
+  to weird hiccup-like jerky motions.
 */
 static void planner_recalculate() 
 {     
@@ -170,7 +197,6 @@ static void planner_recalculate()
 //   if (block_buffer_head != block_buffer_tail) { 
 
   float entry_speed_sqr;
-  float max_entry_speed_sqr;
 
   // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the
   // tail(first) block to prevent over-writing of the initial entry speed. 
@@ -182,24 +208,23 @@ static void planner_recalculate()
     next = current;
     current = &block_buffer[block_index];
     
-    // Determine maximum entry speed at junction. Computed here since feedrate overrides can alter
-    // the planner nominal speeds at any time.
-    max_entry_speed_sqr = current->max_entry_speed_sqr;
-    if (max_entry_speed_sqr > current->nominal_speed_sqr) { max_entry_speed_sqr = current->nominal_speed_sqr; }
-    if (max_entry_speed_sqr > next->nominal_speed_sqr) { max_entry_speed_sqr = next->nominal_speed_sqr; }
+    // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter
+    // the planner nominal speeds at any time. This calc could be done in the override handler, but 
+    // this could require an additional variable to be stored to differentiate the programmed nominal
+    // speeds, max junction speed, and override speeds/scalar.
     
     // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
     // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and 
     // check for maximum allowable speed reductions to ensure maximum possible planned speed.
-    if (current->entry_speed_sqr != max_entry_speed_sqr) {
+    if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
 
-      current->entry_speed_sqr = max_entry_speed_sqr;
+      current->entry_speed_sqr = current->max_entry_speed_sqr;
       current->recalculate_flag = true; // Almost always changes. So force recalculate.       
 
-      if (max_entry_speed_sqr > next->entry_speed_sqr) {
+      if (next->entry_speed_sqr < current->max_entry_speed_sqr) {
         // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance      
         entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
-        if (entry_speed_sqr < max_entry_speed_sqr) {
+        if (entry_speed_sqr < current->max_entry_speed_sqr) {
           current->entry_speed_sqr = entry_speed_sqr;
         }
       } 
@@ -253,15 +278,6 @@ void plan_reset_buffer()
   next_buffer_head = next_block_index(block_buffer_head);
 }
 
-void plan_init() 
-{
-  plan_reset_buffer();
-  memset(&pl, 0, sizeof(pl)); // Clear planner struct
-  pl.inverse_steps_per_mm[X_AXIS] = 1/settings.steps_per_mm[X_AXIS];
-  pl.inverse_steps_per_mm[Y_AXIS] = 1/settings.steps_per_mm[Y_AXIS];
-  pl.inverse_steps_per_mm[Z_AXIS] = 1/settings.steps_per_mm[Z_AXIS];  
-}
-
 inline void plan_discard_current_block() 
 {
   if (block_buffer_head != block_buffer_tail) {
@@ -278,7 +294,11 @@ inline block_t *plan_get_current_block()
 // 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); }
+  if (block_buffer_tail == next_buffer_head) { 
+    // TODO: Move this back into motion control. Shouldn't be here, but it's efficient.
+    if (sys.auto_start) { st_cycle_start(); } // Auto-cycle start when buffer is full.
+    return(true); 
+  }
   return(false);
 }
 
@@ -298,10 +318,11 @@ void plan_synchronize()
 // All position data passed to the planner must be in terms of machine position to keep the planner 
 // independent of any coordinate system changes and offsets, which are handled by the g-code parser.
 // NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
+// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate
+// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate
+// value is negative (and invert_feed_rate always false).
 void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) 
 {
-//   SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
-
   // Prepare to set up new block
   block_t *block = &block_buffer[block_buffer_head];
 
@@ -321,63 +342,55 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
   if (block->step_event_count == 0) { return; };
   
   // Compute path vector in terms of absolute step target and current positions
-  // TODO: Store last xyz in memory to remove steps_per_mm divides. Or come up with another way to save cycles.
-  // Or store pl.inverse_steps_per_mm to speed this up. Would be computed at initialization.
   float delta_mm[3];
-//   delta_mm[X_AXIS] = block->steps_x/settings.steps_per_mm[X_AXIS]; // 110 usec
-//   delta_mm[Y_AXIS] = block->steps_y/settings.steps_per_mm[Y_AXIS];
-//   delta_mm[Z_AXIS] = block->steps_z/settings.steps_per_mm[Z_AXIS];
-  delta_mm[X_AXIS] = block->steps_x*pl.inverse_steps_per_mm[X_AXIS]; // 50usec
-  delta_mm[Y_AXIS] = block->steps_y*pl.inverse_steps_per_mm[Y_AXIS];
-  delta_mm[Z_AXIS] = block->steps_z*pl.inverse_steps_per_mm[Z_AXIS];
+  delta_mm[X_AXIS] = x-pl.last_x;
+  delta_mm[Y_AXIS] = y-pl.last_y; 
+  delta_mm[Z_AXIS] = z-pl.last_z;
   block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + 
                             delta_mm[Z_AXIS]*delta_mm[Z_AXIS]);
 
-  // Compute absolute value of the path unit vector for acceleration calculations.
-  float unit_vec[3];
-  float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides	
-  unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
-  unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
-  unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;  
+  // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
+  // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort.
+  if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later
+  else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
 
-  // Calculate block acceleration based on the maximum possible axes acceleration parameters.
+  // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited
+  // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so
+  // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction
+  // and axes properties as well.
   // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
   // if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
-  // TODO: Install pl.inverse_acceleration to possibly speed up this calculation.
-  float accel_inv_scalar = 0.0;
-  if (unit_vec[X_AXIS]) { accel_inv_scalar = max(accel_inv_scalar,unit_vec[X_AXIS]/settings.acceleration[X_AXIS]); }
-  if (unit_vec[Y_AXIS]) { accel_inv_scalar = max(accel_inv_scalar,unit_vec[Y_AXIS]/settings.acceleration[Y_AXIS]); }
-  if (unit_vec[Z_AXIS]) { accel_inv_scalar = max(accel_inv_scalar,unit_vec[Z_AXIS]/settings.acceleration[Z_AXIS]); }
-  block->acceleration = 1.0/accel_inv_scalar;
+  uint8_t i;
+  float unit_vec[N_AXIS], inverse_unit_vec_value;
+  float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple float divides	
+  block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration
+  for (i=0; i<N_AXIS; i++) { 
+    if (delta_mm[i] == 0) { 
+      unit_vec[i] = 0;  // Store zero value. And avoid divide by zero.
+    } else {
+      // Compute absolute value unit vector
+      unit_vec[i] = delta_mm[i]*inverse_millimeters;
+      inverse_unit_vec_value = abs(1.0/unit_vec[i]);
+      // Check and limit feed rate against max axis velocities and scale accelerations to maximums
+      feed_rate = min(feed_rate,settings.max_velocity[i]*inverse_unit_vec_value);
+      block->acceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value);
+    }
+  }
 
-  // Compute direction bits and apply unit vector directions for junction speed calculations
-  block->direction_bits = 0;
-  if (target[X_AXIS] < pl.position[X_AXIS]) { 
-    block->direction_bits |= (1<<X_DIRECTION_BIT); 
-    unit_vec[X_AXIS] = -unit_vec[X_AXIS];
-  }
-  if (target[Y_AXIS] < pl.position[Y_AXIS]) {
-    block->direction_bits |= (1<<Y_DIRECTION_BIT);
-    unit_vec[Y_AXIS] = -unit_vec[Y_AXIS];
-  }
-  if (target[Z_AXIS] < pl.position[Z_AXIS]) { 
-    block->direction_bits |= (1<<Z_DIRECTION_BIT);
-    unit_vec[Z_AXIS] = -unit_vec[Z_AXIS];
-  }
-  
-  // Calculate speed in mm/minute and stepper rate for each axis. No divide by zero due to previous checks.
-  // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
-  if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
+  // Compute nominal speed and rates
   block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0
   block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
-  
-  // TODO: When acceleration independence is installed, it can be kept in terms of 2*acceleration for 
-  // each block. This could save some 2*acceleration multiplications elsewhere. Need to check though.
-  
+
   // Compute the acceleration and distance traveled per step event for the stepper algorithm.
   block->rate_delta = ceil(block->acceleration*
-    (RANADE_MULTIPLIER/(60.0*60.0*ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
+    ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
   block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
+  
+  // Compute direction bits. Bit enabled always means direction is negative.
+  block->direction_bits = 0;
+  if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
+  if (unit_vec[Y_AXIS] < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
+  if (unit_vec[Z_AXIS] < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
 
   // 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 
@@ -406,22 +419,21 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
                         
     // Skip and use default max junction speed for 0 degree acute junction.
     if (cos_theta < 0.95) {
+      block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr);
       // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
       if (cos_theta > -0.95) {
         // Compute maximum junction velocity based on maximum acceleration and junction deviation
         float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
-        block->max_entry_speed_sqr = block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2);
-      } else {
-        block->max_entry_speed_sqr = 1000000.0; //MAXIMUM_JUNCTION_SPEED;
+        block->max_entry_speed_sqr = min(block->max_entry_speed_sqr,
+           block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2));
       }
     }
   }  
 
   // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED.
-  block->entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters;
-  if (block->entry_speed_sqr > pl.previous_nominal_speed_sqr) { block->entry_speed_sqr = pl.previous_nominal_speed_sqr; }
-  if (block->entry_speed_sqr > block->nominal_speed_sqr) { block->entry_speed_sqr = block->nominal_speed_sqr; }
-  if (block->entry_speed_sqr > block->max_entry_speed_sqr) { block->entry_speed_sqr = block->max_entry_speed_sqr; }
+  // TODO: This could be moved to the planner recalculate function.
+  block->entry_speed_sqr = min( block->max_entry_speed_sqr,
+      MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters);
 
   // Set new block to be recalculated for conversion to stepper data.
   block->recalculate_flag = true;
@@ -429,24 +441,29 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
   // Update previous path unit_vector and nominal speed (squared)
   memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
   pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
-  
+    
+  // Update planner position
+  memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]
+  pl.last_x = x;
+  pl.last_y = y;
+  pl.last_z = z;
+
   // Update buffer head and next buffer head indices
   block_buffer_head = next_buffer_head;  
   next_buffer_head = next_block_index(block_buffer_head);
-  
-  // Update planner position
-  memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]
 
   planner_recalculate(); 
-//    SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;
 }
 
-// Reset the planner position vector (in steps). Called by the system abort routine.
+// Reset the planner position vectors. Called by the system abort routine.
 void plan_set_current_position(int32_t x, int32_t y, int32_t z)
 {
   pl.position[X_AXIS] = x;
   pl.position[Y_AXIS] = y;
   pl.position[Z_AXIS] = z;
+  pl.last_x = x/settings.steps_per_mm[X_AXIS];
+  pl.last_y = y/settings.steps_per_mm[Y_AXIS];
+  pl.last_z = z/settings.steps_per_mm[Z_AXIS];
 }
 
 // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
diff --git a/report.c b/report.c
index 8ed7881..a27f8c1 100644
--- a/report.c
+++ b/report.c
@@ -146,30 +146,32 @@ void report_grbl_settings() {
   printPgmString(PSTR("$0=")); printFloat(settings.steps_per_mm[X_AXIS]);
   printPgmString(PSTR(" (x, step/mm)\r\n$1=")); printFloat(settings.steps_per_mm[Y_AXIS]);
   printPgmString(PSTR(" (y, step/mm)\r\n$2=")); printFloat(settings.steps_per_mm[Z_AXIS]);
-  printPgmString(PSTR(" (z, step/mm)\r\n$3=")); printInteger(settings.pulse_microseconds);
-  printPgmString(PSTR(" (step pulse, usec)\r\n$4=")); printFloat(settings.default_feed_rate);
-  printPgmString(PSTR(" (default feed, mm/min)\r\n$5=")); printFloat(settings.default_seek_rate);
-  printPgmString(PSTR(" (default seek, mm/min)\r\n$6=")); printInteger(settings.invert_mask); 
+  printPgmString(PSTR(" (z, step/mm)\r\n$3=")); printFloat(settings.max_velocity[X_AXIS]);
+  printPgmString(PSTR(" (x v_max, mm/min)\r\n$4=")); printFloat(settings.max_velocity[Y_AXIS]);
+  printPgmString(PSTR(" (y v_max, mm/min)\r\n$5=")); printFloat(settings.max_velocity[Z_AXIS]);
+  printPgmString(PSTR(" (z v_max, mm/min)\r\n$6=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
+  printPgmString(PSTR(" (x accel, mm/sec^2)\r\n$7=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
+  printPgmString(PSTR(" (y accel, mm/sec^2)\r\n$8=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
+  printPgmString(PSTR(" (z accel, mm/sec^2)\r\n$9=")); printInteger(settings.pulse_microseconds);
+  printPgmString(PSTR(" (step pulse, usec)\r\n$10=")); printFloat(settings.default_feed_rate);
+  printPgmString(PSTR(" (default feed, mm/min)\r\n$11=")); printInteger(settings.invert_mask); 
   printPgmString(PSTR(" (step port invert mask, int:")); print_uint8_base2(settings.invert_mask);  
-  printPgmString(PSTR(")\r\n$7=")); printInteger(settings.stepper_idle_lock_time);
-  printPgmString(PSTR(" (step idle delay, msec)\r\n$8=")); printFloat(settings.acceleration[X_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
-  printPgmString(PSTR(" (x acceleration, mm/sec^2)\r\n$9=")); printFloat(settings.acceleration[Y_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
-  printPgmString(PSTR(" (y acceleration, mm/sec^2)\r\n$10=")); printFloat(settings.acceleration[Z_AXIS]/(60*60)); // Convert from mm/min^2 for human readability
-  printPgmString(PSTR(" (z acceleration, mm/sec^2)\r\n$11=")); printFloat(settings.junction_deviation);
-  printPgmString(PSTR(" (junction deviation, mm)\r\n$12=")); printFloat(settings.mm_per_arc_segment);
-  printPgmString(PSTR(" (arc, mm/segment)\r\n$13=")); printInteger(settings.n_arc_correction);
-  printPgmString(PSTR(" (n-arc correction, int)\r\n$14=")); printInteger(settings.decimal_places);
-  printPgmString(PSTR(" (n-decimals, int)\r\n$15=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES));
-  printPgmString(PSTR(" (report inches, bool)\r\n$16=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START));
-  printPgmString(PSTR(" (auto start, bool)\r\n$17=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE));
-  printPgmString(PSTR(" (invert step enable, bool)\r\n$18=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE));
-  printPgmString(PSTR(" (hard limits, bool)\r\n$19=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE));
-  printPgmString(PSTR(" (homing cycle, bool)\r\n$20=")); printInteger(settings.homing_dir_mask);
+  printPgmString(PSTR(")\r\n$12=")); printInteger(settings.stepper_idle_lock_time);
+  printPgmString(PSTR(" (step idle delay, msec)\r\n$13=")); printFloat(settings.junction_deviation);
+  printPgmString(PSTR(" (junction deviation, mm)\r\n$14=")); printFloat(settings.mm_per_arc_segment);
+  printPgmString(PSTR(" (arc, mm/segment)\r\n$15=")); printInteger(settings.n_arc_correction);
+  printPgmString(PSTR(" (n-arc correction, int)\r\n$16=")); printInteger(settings.decimal_places);
+  printPgmString(PSTR(" (n-decimals, int)\r\n$17=")); printInteger(bit_istrue(settings.flags,BITFLAG_REPORT_INCHES));
+  printPgmString(PSTR(" (report inches, bool)\r\n$18=")); printInteger(bit_istrue(settings.flags,BITFLAG_AUTO_START));
+  printPgmString(PSTR(" (auto start, bool)\r\n$19=")); printInteger(bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE));
+  printPgmString(PSTR(" (invert step enable, bool)\r\n$20=")); printInteger(bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE));
+  printPgmString(PSTR(" (hard limits, bool)\r\n$21=")); printInteger(bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE));
+  printPgmString(PSTR(" (homing cycle, bool)\r\n$22=")); printInteger(settings.homing_dir_mask);
   printPgmString(PSTR(" (homing dir invert mask, int:")); print_uint8_base2(settings.homing_dir_mask);  
-  printPgmString(PSTR(")\r\n$21=")); printFloat(settings.homing_feed_rate);
-  printPgmString(PSTR(" (homing feed, mm/min)\r\n$22=")); printFloat(settings.homing_seek_rate);
-  printPgmString(PSTR(" (homing seek, mm/min)\r\n$23=")); printInteger(settings.homing_debounce_delay);
-  printPgmString(PSTR(" (homing debounce, msec)\r\n$24=")); printFloat(settings.homing_pulloff);
+  printPgmString(PSTR(")\r\n$23=")); printFloat(settings.homing_feed_rate);
+  printPgmString(PSTR(" (homing feed, mm/min)\r\n$24=")); printFloat(settings.homing_seek_rate);
+  printPgmString(PSTR(" (homing seek, mm/min)\r\n$25=")); printInteger(settings.homing_debounce_delay);
+  printPgmString(PSTR(" (homing debounce, msec)\r\n$26=")); printFloat(settings.homing_pulloff);
   printPgmString(PSTR(" (homing pull-off, mm)\r\n")); 
 }
 
diff --git a/settings.c b/settings.c
index 1f99847..6f7d30a 100644
--- a/settings.c
+++ b/settings.c
@@ -36,7 +36,6 @@ typedef struct {
   uint8_t microsteps;
   uint8_t pulse_microseconds;
   float default_feed_rate;
-  float default_seek_rate;
   uint8_t invert_mask;
   float mm_per_arc_segment;
   float acceleration;
@@ -74,7 +73,9 @@ void settings_reset(bool reset_all) {
     settings.steps_per_mm[Z_AXIS] = DEFAULT_Z_STEPS_PER_MM;
     settings.pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS;
     settings.default_feed_rate = DEFAULT_FEEDRATE;
-    settings.default_seek_rate = DEFAULT_RAPID_FEEDRATE;
+    settings.max_velocity[X_AXIS] = DEFAULT_RAPID_FEEDRATE;
+    settings.max_velocity[Y_AXIS] = DEFAULT_RAPID_FEEDRATE;
+    settings.max_velocity[Z_AXIS] = DEFAULT_RAPID_FEEDRATE;
     settings.acceleration[X_AXIS] = DEFAULT_ACCELERATION;
     settings.acceleration[Y_AXIS] = DEFAULT_ACCELERATION;
     settings.acceleration[Z_AXIS] = DEFAULT_ACCELERATION;
@@ -159,46 +160,48 @@ uint8_t settings_store_global_setting(int parameter, float value) {
     case 0: case 1: case 2:
       if (value <= 0.0) { return(STATUS_SETTING_VALUE_NEG); } 
       settings.steps_per_mm[parameter] = value; break;
-    case 3: 
+    case 3: settings.max_velocity[X_AXIS] = value; break;
+    case 4: settings.max_velocity[Y_AXIS] = value; break;
+    case 5: settings.max_velocity[Z_AXIS] = value; break;
+    case 6: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
+    case 7: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
+    case 8: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
+    case 9: 
       if (value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); }
       settings.pulse_microseconds = round(value); break;
-    case 4: settings.default_feed_rate = value; break;
-    case 5: settings.default_seek_rate = value; break;
-    case 6: settings.invert_mask = trunc(value); break;
-    case 7: settings.stepper_idle_lock_time = round(value); break;
-    case 8: settings.acceleration[X_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
-    case 9: settings.acceleration[Y_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
-    case 10: settings.acceleration[Z_AXIS] = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
-    case 11: settings.junction_deviation = fabs(value); break;
-    case 12: settings.mm_per_arc_segment = value; break;
-    case 13: settings.n_arc_correction = round(value); break;
-    case 14: settings.decimal_places = round(value); break;
-    case 15:
+    case 10: settings.default_feed_rate = value; break;
+    case 11: settings.invert_mask = trunc(value); break;
+    case 12: settings.stepper_idle_lock_time = round(value); break;
+    case 13: settings.junction_deviation = fabs(value); break;
+    case 14: settings.mm_per_arc_segment = value; break;
+    case 15: settings.n_arc_correction = round(value); break;
+    case 16: settings.decimal_places = round(value); break;
+    case 17:
       if (value) { settings.flags |= BITFLAG_REPORT_INCHES; }
       else { settings.flags &= ~BITFLAG_REPORT_INCHES; }
       break;
-    case 16: // Reset to ensure change. Immediate re-init may cause problems.
+    case 18: // Reset to ensure change. Immediate re-init may cause problems.
       if (value) { settings.flags |= BITFLAG_AUTO_START; }
       else { settings.flags &= ~BITFLAG_AUTO_START; }
       break;
-    case 17: // Reset to ensure change. Immediate re-init may cause problems.
+    case 19: // Reset to ensure change. Immediate re-init may cause problems.
       if (value) { settings.flags |= BITFLAG_INVERT_ST_ENABLE; }
       else { settings.flags &= ~BITFLAG_INVERT_ST_ENABLE; }
       break;
-    case 18:
+    case 20:
       if (value) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; }
       else { settings.flags &= ~BITFLAG_HARD_LIMIT_ENABLE; }
       limits_init(); // Re-init to immediately change. NOTE: Nice to have but could be problematic later.
       break;
-    case 19:
+    case 21:
       if (value) { settings.flags |= BITFLAG_HOMING_ENABLE; }
       else { settings.flags &= ~BITFLAG_HOMING_ENABLE; }
       break;
-    case 20: settings.homing_dir_mask = trunc(value); break;
-    case 21: settings.homing_feed_rate = value; break;
-    case 22: settings.homing_seek_rate = value; break;
-    case 23: settings.homing_debounce_delay = round(value); break;
-    case 24: settings.homing_pulloff = value; break;
+    case 22: settings.homing_dir_mask = trunc(value); break;
+    case 23: settings.homing_feed_rate = value; break;
+    case 24: settings.homing_seek_rate = value; break;
+    case 25: settings.homing_debounce_delay = round(value); break;
+    case 26: settings.homing_pulloff = value; break;
     default: 
       return(STATUS_INVALID_STATEMENT);
   }
diff --git a/settings.h b/settings.h
index cce9222..1b7e32e 100644
--- a/settings.h
+++ b/settings.h
@@ -29,7 +29,7 @@
 
 // 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
-#define SETTINGS_VERSION 50
+#define SETTINGS_VERSION 51
 
 // Define bit flag masks for the boolean settings in settings.flag.
 #define BITFLAG_REPORT_INCHES      bit(0)
@@ -74,6 +74,8 @@ typedef struct {
   uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable.
   uint8_t decimal_places;
   uint8_t n_arc_correction;
+  float max_velocity[N_AXIS];
+//   float mm_soft_limit[N_AXIS];
 //  uint8_t status_report_mask; // Mask to indicate desired report data.
 } settings_t;
 extern settings_t settings;
diff --git a/stepper.c b/stepper.c
index a35a19d..4dcae3b 100644
--- a/stepper.c
+++ b/stepper.c
@@ -47,7 +47,7 @@ typedef struct {
   // Used by Pramod Ranade inverse time algorithm
   int32_t delta_d;     // Ranade distance traveled per interrupt tick
   int32_t d_counter;   // Ranade distance traveled since last step event
-  uint16_t ramp_count; // Acceleration interrupt tick counter. 
+  uint32_t ramp_count; // Acceleration interrupt tick counter. 
   uint8_t ramp_type; // Ramp type variable.
   uint8_t execute_step; // Flags step execution for each interrupt.
   
@@ -144,10 +144,8 @@ ISR(TIMER2_COMPA_vect)
 // SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
   if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
   
-  // Set direction pins. New block dir will always be set one timer tick before any step pulse.
-  STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
-
-  // Pulse stepper pins, if flagged.
+  // Pulse stepper port pins, if flagged. New block dir will always be set one timer tick 
+  // before any step pulse due to algorithm design.
   if (st.execute_step) {
     st.execute_step = false;
     STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | out_bits;
@@ -170,8 +168,9 @@ ISR(TIMER2_COMPA_vect)
       // of a block, hence helping minimize total time spent in this interrupt.
 
       // Initialize direction bits for block      
-      out_bits = current_block->direction_bits ^ (settings.invert_mask & DIRECTION_MASK);
-            
+      out_bits = current_block->direction_bits ^ settings.invert_mask;
+      st.execute_step = true; // Set flag to set direction bits.
+      
       // Initialize Bresenham variables
       st.counter_x = (current_block->step_event_count >> 1);
       st.counter_y = st.counter_x;
@@ -216,7 +215,7 @@ ISR(TIMER2_COMPA_vect)
         if (st.delta_d > current_block->rate_delta) {
           st.delta_d -= current_block->rate_delta;
         } else {
-          st.delta_d = MINIMUM_STEP_RATE; // Prevent integer overflow
+          st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
         }
       }
     }
@@ -278,10 +277,14 @@ ISR(TIMER2_COMPA_vect)
       if (st.ramp_type != DECEL_RAMP) {
         // Acceleration and cruise handled by ramping. Just check for deceleration.
         if (st.step_events_remaining <=  current_block->decelerate_after) {
-          if (st.step_events_remaining == current_block->decelerate_after) { 
-            st.ramp_count = INTERRUPTS_PER_ACCELERATION_TICK/2; 
-          }
           st.ramp_type = DECEL_RAMP;
+          if (st.step_events_remaining == current_block->decelerate_after) {
+            if (st.delta_d == current_block->nominal_rate) {
+              st.ramp_count = INTERRUPTS_PER_ACCELERATION_TICK/2;  // Set ramp counter for trapezoid
+            } else {
+              st.ramp_count = INTERRUPTS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
+            }
+          }
         }
       }
     } else {
@@ -302,7 +305,7 @@ ISR(TIMER2_COMPA_vect)
 ISR(TIMER0_OVF_vect)
 {
   STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
-  TCNT0 = 0; // Disable timer until needed.
+  TCCR0B = 0; // Disable timer until needed.
 }