Merge chamnit/v0_7 with grbl/master

Makefile

@@ -30,8 +30,8 @@
 DEVICE     = atmega328p
 CLOCK      = 16000000
 PROGRAMMER = -c avrisp2 -P usb
-OBJECTS    = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o \
+OBJECTS    = main.o motion_control.o gcode.o spindle_control.o serial.o protocol.o stepper.o \
-             eeprom.o settings.o planner.o
+             eeprom.o settings.o planner.o nuts_bolts.o limits.o print.o
 # FUSES      = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
 FUSES      = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m
 # update that line with this when programmer is back up: 

config.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
@@ -21,14 +22,16 @@
 #ifndef config_h
 #define config_h
 
+// IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them.
+
 #define BAUD_RATE 9600
 
 // Updated default pin-assignments from 0.6 onwards 
 // (see bottom of file for a copy of the old config)
 
-#define STEPPERS_ENABLE_DDR     DDRB
+#define STEPPERS_DISABLE_DDR     DDRB
-#define STEPPERS_ENABLE_PORT    PORTB
+#define STEPPERS_DISABLE_PORT    PORTB
-#define STEPPERS_ENABLE_BIT         0
+#define STEPPERS_DISABLE_BIT         0
 
 #define STEPPING_DDR       DDRD
 #define STEPPING_PORT      PORTD
@@ -40,7 +43,7 @@
 #define Z_DIRECTION_BIT      7
 
 #define LIMIT_DDR      DDRB
-#define LIMIT_PORT     PORTB
+#define LIMIT_PIN     PINB
 #define X_LIMIT_BIT          1
 #define Y_LIMIT_BIT          2
 #define Z_LIMIT_BIT          3
@@ -53,17 +56,51 @@
 #define SPINDLE_DIRECTION_PORT PORTB
 #define SPINDLE_DIRECTION_BIT 5
 
-// The temporal resolution of the acceleration management subsystem. Higher number
+// This parameter sets the delay time before disabling the steppers after the final block of movement.
-// give smoother acceleration but may impact performance
+// A short delay ensures the steppers come to a complete stop and the residual inertial force in the 
-#define ACCELERATION_TICKS_PER_SECOND 40L
+// CNC axes don't cause the axes to drift off position. This is particularly important when manually 
+// entering g-code into grbl, i.e. locating part zero or simple manual machining. If the axes drift,
+// grbl has no way to know this has happened, since stepper motors are open-loop control. Depending
+// on the machine, this parameter may need to be larger or smaller than the default time.
+// NOTE: If defined 0, the delay will not be compiled.
+#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) - Integer >= 0
+
+// The temporal resolution of the acceleration management subsystem. Higher number give smoother
+// acceleration but may impact performance.
+// NOTE: Increasing this parameter will help any resolution related issues, especially with machines 
+// requiring very high accelerations and/or very fast feedrates. In general, this will reduce the 
+// error between how the planner plans the motions and how the stepper program actually performs them.
+// However, at some point, the resolution can be high enough, where the errors related to numerical 
+// round-off can be great enough to cause problems and/or it's too fast for the Arduino. The correct
+// value for this parameter is machine dependent, so it's advised to set this only as high as needed.
+// Approximate successful values can range from 30L to 100L or more.
+#define ACCELERATION_TICKS_PER_SECOND 50L
+
+// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end
+// of the buffer and all stops. This should not be much greater than zero and should only be changed
+// if unwanted behavior is observed on a user's machine when running at very slow speeds.
+#define MINIMUM_PLANNER_SPEED 0.0 // (mm/min)
+
+// Minimum stepper rate. Sets the absolute minimum stepper rate in the stepper program and never runs
+// slower than this value, except when sleeping. This parameter overrides the minimum planner speed.
+// This is primarily used to guarantee that the end of a movement is always reached and not stop to
+// never reach its target. This parameter should always be greater than zero.
+#define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only
+
+// Number of arc generation iterations by small angle approximation before exact arc trajectory 
+// correction. This parameter maybe decreased if there are issues with the accuracy of the arc
+// generations. In general, the default value is more than enough for the intended CNC applications
+// of grbl, and should be on the order or greater than the size of the buffer to help with the 
+// computational efficiency of generating arcs.
+#define N_ARC_CORRECTION 25 // Integer (1-255)
 
 #endif
 
 // Pin-assignments from Grbl 0.5
 
-// #define STEPPERS_ENABLE_DDR     DDRD
+// #define STEPPERS_DISABLE_DDR     DDRD
-// #define STEPPERS_ENABLE_PORT    PORTD
+// #define STEPPERS_DISABLE_PORT    PORTD
-// #define STEPPERS_ENABLE_BIT         2
+// #define STEPPERS_DISABLE_BIT         2
 // 
 // #define STEPPING_DDR       DDRC
 // #define STEPPING_PORT      PORTC 
@@ -87,4 +124,3 @@
 // #define SPINDLE_DIRECTION_DDR DDRD
 // #define SPINDLE_DIRECTION_PORT PORTD
 // #define SPINDLE_DIRECTION_BIT 7
-

doc/planner-maths.txt

@@ -0,0 +1,30 @@
+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)

doc/resources.txt

@@ -3,7 +3,7 @@ Allocation of AVR peripherals in Grbl
 
 See config.h for pin allocation.
 
-The UART is handled by 'wiring_serial' and used primarily for streaming gcode
+The UART is handled by 'serial' and used primarily for streaming gcode
 
 16 bit Timer 1 and the TIMER1_COMPA interrupt is used by the 'stepper' module to handle step events
 

doc/structure.txt

@@ -3,10 +3,10 @@ The general structure of Grbl
 
 The main processing stack:
 
-'serial_protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution.
+'protocol'        : Accepts command lines from the serial port and passes them to 'gcode' for execution.
                     Provides status responses for each command.
                   
-'gcode'           : Recieves gcode from 'serial_protocol', parses it according to the current state
+'gcode'           : Recieves gcode from 'protocol', parses it according to the current state
                     of the parser and issues commands via '..._control' modules
                   
 'spindle_control' : Commands for controlling the spindle.
@@ -36,4 +36,6 @@ Supporting files:
                     
 'nuts_bolts.h'    : A tiny collection of useful constants and macros used everywhere
 
-'wiring_serial'   : Low level serial library initially from an old version of the Arduino software
+'serial'          : Low level serial communications
+
+'print'           : Functions to print strings of different formats (using serial)

gcode.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
@@ -22,7 +23,6 @@
    by Kramer, Proctor and Messina. */
 
 #include "gcode.h"
-#include <stdlib.h>
 #include <string.h>
 #include "nuts_bolts.h"
 #include <math.h>
@@ -30,13 +30,14 @@
 #include "motion_control.h"
 #include "spindle_control.h"
 #include "errno.h"
-#include "serial_protocol.h"
+#include "protocol.h"
 
 #define MM_PER_INCH (25.4)
 
 #define NEXT_ACTION_DEFAULT 0
 #define NEXT_ACTION_DWELL 1
 #define NEXT_ACTION_GO_HOME 2
+#define NEXT_ACTION_SET_COORDINATE_OFFSET 3
 
 #define MOTION_MODE_SEEK 0 // G0 
 #define MOTION_MODE_LINEAR 1 // G1
@@ -63,7 +64,7 @@ typedef struct {
   uint8_t inches_mode;             /* 0 = millimeter mode, 1 = inches mode {G20, G21} */
   uint8_t absolute_mode;           /* 0 = relative motion, 1 = absolute motion {G90, G91} */
   uint8_t program_flow;
-  int spindle_direction;
+  int8_t spindle_direction;
   double feed_rate, seek_rate;     /* Millimeters/second */
   double position[3];              /* Where the interpreter considers the tool to be at this point in the code */
   uint8_t tool;
@@ -76,14 +77,9 @@ static parser_state_t gc;
 
 #define FAIL(status) gc.status_code = status;
 
-int read_double(char *line,               //  <- string: line of RS274/NGC code being processed
+static int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter);
-                     int *char_counter,        //  <- pointer to a counter for position on the line 
-                     double *double_ptr); //  <- pointer to double to be read                  
 
-int next_statement(char *letter, double *double_ptr, char *line, int *char_counter);
+static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) 
-
-
-void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) 
 {
   gc.plane_axis_0 = axis_0;
   gc.plane_axis_1 = axis_1;
@@ -92,44 +88,28 @@ 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/60;
+  gc.feed_rate = settings.default_feed_rate;
-  gc.seek_rate = settings.default_seek_rate/60;
+  gc.seek_rate = settings.default_seek_rate;
   select_plane(X_AXIS, Y_AXIS, Z_AXIS);
-  gc.absolute_mode = TRUE;
+  gc.absolute_mode = true;
 }
 
-inline float to_millimeters(double value) {
+static float to_millimeters(double value) {
   return(gc.inches_mode ? (value * MM_PER_INCH) : value);
 }
 
-// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, 
-// positive to the right.
-double theta(double x, double y)
-{
-  double theta = atan(x/fabs(y));
-  if (y>0) {
-    return(theta);
-  } else {
-    if (theta>0) 
-    {
-      return(M_PI-theta);
-    } else {
-      return(-M_PI-theta);
-    }
-  }
-}
-
 // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
-// characters and signed floating point values (no whitespace).
+// characters and signed floating point values (no whitespace). Comments and block delete
+// characters have been removed.
 uint8_t gc_execute_line(char *line) {
-  int char_counter = 0;  
+  uint8_t char_counter = 0;  
   char letter;
   double value;
   double unit_converted_value;
   double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
-  int radius_mode = FALSE;
+  uint8_t radius_mode = false;
   
-  uint8_t absolute_override = FALSE;          /* 1 = absolute motion for this block only {G53} */
+  uint8_t absolute_override = false;          /* 1 = absolute motion for this block only {G53} */
   uint8_t next_action = NEXT_ACTION_DEFAULT;  /* The action that will be taken by the parsed line */
   
   double target[3], offset[3];  
@@ -137,29 +117,7 @@ uint8_t gc_execute_line(char *line) {
   double p = 0, r = 0;
   int int_value;
 
-  clear_vector(target);
+  gc.status_code = STATUS_OK;
-  clear_vector(offset);
-
-  gc.status_code = GCSTATUS_OK;
-  
-  // Disregard comments and block delete
-  if (line[0] == '(') { return(gc.status_code); }
-  if (line[0] == '/') { char_counter++; } // ignore block delete  
-  
-  // If the line starts with an '$' it is a configuration-command
-  if (line[0] == '$') { 
-    // Parameter lines are on the form '$4=374.3' or '$' to dump current settings
-    char_counter = 1;
-    if(line[char_counter] == 0) { settings_dump(); return(GCSTATUS_OK); }
-    read_double(line, &char_counter, &p);
-    if(line[char_counter++] != '=') { return(GCSTATUS_UNSUPPORTED_STATEMENT); }
-    read_double(line, &char_counter, &value);
-    if(line[char_counter] != 0) { return(GCSTATUS_UNSUPPORTED_STATEMENT); }
-    settings_store_setting(p, value);
-    return(gc.status_code);
-  }
-  
-  /* We'll handle this as g-code. First: parse all statements */
   
   // Pass 1: Commands
   while(next_statement(&letter, &value, line, &char_counter)) {
@@ -177,16 +135,17 @@ uint8_t gc_execute_line(char *line) {
         case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
         case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
         case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
-        case 20: gc.inches_mode = TRUE; break;
+        case 20: gc.inches_mode = true; break;
-        case 21: gc.inches_mode = FALSE; break;
+        case 21: gc.inches_mode = false; break;
         case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
-        case 53: absolute_override = TRUE; break;
+        case 53: absolute_override = true; break;
         case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;        
-        case 90: gc.absolute_mode = TRUE; break;
+        case 90: gc.absolute_mode = true; break;
-        case 91: gc.absolute_mode = FALSE; break;
+        case 91: gc.absolute_mode = false; break;
-        case 93: gc.inverse_feed_rate_mode = TRUE; break;
+        case 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;        
-        case 94: gc.inverse_feed_rate_mode = FALSE; break;
+        case 93: gc.inverse_feed_rate_mode = true; break;
-        default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
+        case 94: gc.inverse_feed_rate_mode = false; break;
+        default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
       }
       break;
       
@@ -197,7 +156,7 @@ uint8_t gc_execute_line(char *line) {
         case 3: gc.spindle_direction = 1; break;
         case 4: gc.spindle_direction = -1; break;
         case 5: gc.spindle_direction = 0; break;
-        default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
+        default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
       }            
       break;
       case 'T': gc.tool = trunc(value); break;
@@ -209,6 +168,7 @@ uint8_t gc_execute_line(char *line) {
   if (gc.status_code) { return(gc.status_code); }
 
   char_counter = 0;
+  clear_vector(target);
   clear_vector(offset);
   memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
 
@@ -218,19 +178,20 @@ uint8_t gc_execute_line(char *line) {
     unit_converted_value = to_millimeters(value);
     switch(letter) {
       case 'F': 
+      if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // Must be greater than zero
       if (gc.inverse_feed_rate_mode) {
         inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
       } else {          
         if (gc.motion_mode == MOTION_MODE_SEEK) {
-          gc.seek_rate = unit_converted_value/60;
+          gc.seek_rate = unit_converted_value;
         } else {
-          gc.feed_rate = unit_converted_value/60; // millimeters pr second
+          gc.feed_rate = unit_converted_value; // millimeters per minute
         }
       }
       break;
       case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
       case 'P': p = value; break;
-      case 'R': r = unit_converted_value; radius_mode = TRUE; break;
+      case 'R': r = unit_converted_value; radius_mode = true; break;
       case 'S': gc.spindle_speed = value; break;
       case 'X': case 'Y': case 'Z':
       if (gc.absolute_mode || absolute_override) {
@@ -246,21 +207,20 @@ uint8_t gc_execute_line(char *line) {
   if (gc.status_code) { return(gc.status_code); }
     
   // Update spindle state
-  if (gc.spindle_direction) {
   spindle_run(gc.spindle_direction, gc.spindle_speed);
-  } else {
-    spindle_stop();
-  }
   
   // Perform any physical actions
   switch (next_action) {
-    case NEXT_ACTION_GO_HOME: mc_go_home(); break;
+    case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(target); break;
-    case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break;
+    case NEXT_ACTION_DWELL: mc_dwell(p); break;   
+    case NEXT_ACTION_SET_COORDINATE_OFFSET: 
+    mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]);
+    break;
     case NEXT_ACTION_DEFAULT: 
     switch (gc.motion_mode) {
       case MOTION_MODE_CANCEL: break;
       case MOTION_MODE_SEEK:
-      mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, FALSE);
+      mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false);
       break;
       case MOTION_MODE_LINEAR:
       mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], 
@@ -327,7 +287,7 @@ uint8_t gc_execute_line(char *line) {
         double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
         // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
         // real CNC, and thus - for practical reasons - we will terminate promptly:
-        if(isnan(h_x2_div_d)) { FAIL(GCSTATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
+        if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
         // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
         if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
         
@@ -352,50 +312,29 @@ uint8_t gc_execute_line(char *line) {
         // even though it is advised against ever generating such circles in a single line of g-code. By 
         // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
         // travel and thus we get the unadvisably long arcs as prescribed.
-        if (r < 0) { h_x2_div_d = -h_x2_div_d; }        
+        if (r < 0) { 
+            h_x2_div_d = -h_x2_div_d; 
+            r = -r; // Finished with r. Set to positive for mc_arc
+        }        
         // Complete the operation by calculating the actual center of the arc
-        offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2;
+        offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
-        offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2;
+        offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
+
+      } else { // Offset mode specific computations
+
+        r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
+
       }
       
-      /*
+      // Set clockwise/counter-clockwise sign for mc_arc computations
-         This segment sets up an clockwise or counterclockwise arc from the current position to the target position around 
+      uint8_t isclockwise = false;
-         the center designated by the offset vector. All theta-values measured in radians of deviance from the positive 
+      if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
-         y-axis. 
 
-                            | <- theta == 0
-                          * * *                
-                        *       *                                               
-                      *           *                                             
-                      *     O ----T   <- theta_end (e.g. 90 degrees: theta_end == PI/2)                                          
-                      *   /                                                     
-                        C   <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
-
-      */
-            
-      // calculate the theta (angle) of the current point
-      double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]);
-      // calculate the theta (angle) of the target point
-      double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0], 
-         target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.plane_axis_1]);
-      // ensure that the difference is positive so that we have clockwise travel
-      if (theta_end < theta_start) { theta_end += 2*M_PI; }
-      double angular_travel = theta_end-theta_start;
-      // Invert angular motion if the g-code wanted a counterclockwise arc
-      if (gc.motion_mode == MOTION_MODE_CCW_ARC) {
-        angular_travel = angular_travel-2*M_PI;
-      }
-      // Find the radius
-      double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
-      // Calculate the motion along the depth axis of the helix
-      double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2];
       // Trace the arc
-      mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, 
+      mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
         (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
-        gc.position);
+        r, isclockwise);
-      // Finish off with a line to make sure we arrive exactly where we think we are
+        
-      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;
 #endif      
     }    
@@ -411,40 +350,24 @@ uint8_t gc_execute_line(char *line) {
 // Parses the next statement and leaves the counter on the first character following
 // the statement. Returns 1 if there was a statements, 0 if end of string was reached
 // or there was an error (check state.status_code).
-int next_statement(char *letter, double *double_ptr, char *line, int *char_counter) {
+static int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter) {
   if (line[*char_counter] == 0) {
     return(0); // No more statements
   }
   
   *letter = line[*char_counter];
   if((*letter < 'A') || (*letter > 'Z')) {
-    FAIL(GCSTATUS_EXPECTED_COMMAND_LETTER);
+    FAIL(STATUS_EXPECTED_COMMAND_LETTER);
     return(0);
   }
   (*char_counter)++;
   if (!read_double(line, char_counter, double_ptr)) {
+    FAIL(STATUS_BAD_NUMBER_FORMAT); 
     return(0);
   };
   return(1);
 }
 
-int read_double(char *line,               //!< string: line of RS274/NGC code being processed
-                     int *char_counter,   //!< pointer to a counter for position on the line 
-                     double *double_ptr)  //!< pointer to double to be read                  
-{
-  char *start = line + *char_counter;
-  char *end;
-  
-  *double_ptr = strtod(start, &end);
-  if(end == start) { 
-    FAIL(GCSTATUS_BAD_NUMBER_FORMAT); 
-    return(0); 
-  };
-
-  *char_counter = end - line;
-  return(1);
-}
-
 /* 
   Intentionally not supported:
 
@@ -464,4 +387,3 @@ int read_double(char *line,               //!< string: line of RS274/NGC code be
    group 12 = {G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3} coordinate system selection
    group 13 = {G61, G61.1, G64} path control mode
 */
-

gcode.h

@@ -23,12 +23,6 @@
 #define gcode_h
 #include <avr/io.h>
 
-#define GCSTATUS_OK 0
-#define GCSTATUS_BAD_NUMBER_FORMAT 1
-#define GCSTATUS_EXPECTED_COMMAND_LETTER 2
-#define GCSTATUS_UNSUPPORTED_STATEMENT 3
-#define GCSTATUS_FLOATING_POINT_ERROR 4
-
 // Initialize the parser
 void gc_init();
 

limits.c

@@ -0,0 +1,103 @@
+/*
+  limits.h - code pertaining to limit-switches and performing the homing cycle
+  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 <util/delay.h>
+#include <avr/io.h>
+#include "stepper.h"
+#include "settings.h"
+#include "nuts_bolts.h"
+#include "config.h"
+
+void limits_init() {
+  LIMIT_DDR &= ~(LIMIT_MASK);
+}
+
+static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool reverse_direction, uint32_t microseconds_per_pulse) {
+  // First home the Z axis
+  uint32_t step_delay = microseconds_per_pulse - settings.pulse_microseconds;
+  uint8_t out_bits = DIRECTION_MASK;
+  uint8_t limit_bits;
+  
+  if (x_axis) { out_bits |= (1<<X_STEP_BIT); }
+  if (y_axis) { out_bits |= (1<<Y_STEP_BIT); }
+  if (z_axis) { out_bits |= (1<<Z_STEP_BIT); }
+  
+  // Invert direction bits if this is a reverse homing_cycle
+  if (reverse_direction) {
+    out_bits ^= DIRECTION_MASK;
+  }
+  
+  // Apply the global invert mask
+  out_bits ^= settings.invert_mask;
+  
+  // Set direction pins
+  STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
+  
+  for(;;) {
+    limit_bits = LIMIT_PIN;
+    if (reverse_direction) {         
+      // Invert limit_bits if this is a reverse homing_cycle
+      limit_bits ^= LIMIT_MASK;
+    }
+    if (x_axis && !(LIMIT_PIN & (1<<X_LIMIT_BIT))) {
+      x_axis = false;
+      out_bits ^= (1<<X_STEP_BIT);      
+    }    
+    if (y_axis && !(LIMIT_PIN & (1<<Y_LIMIT_BIT))) {
+      y_axis = false;
+      out_bits ^= (1<<Y_STEP_BIT);
+    }    
+    if (z_axis && !(LIMIT_PIN & (1<<Z_LIMIT_BIT))) {
+      z_axis = false;
+      out_bits ^= (1<<Z_STEP_BIT);
+    }
+    // Check if we are done
+    if(!(x_axis || y_axis || z_axis)) { return; }
+    STEPPING_PORT |= out_bits & STEP_MASK;
+    _delay_us(settings.pulse_microseconds);
+    STEPPING_PORT ^= out_bits & STEP_MASK;
+    _delay_us(step_delay);
+  }
+  return;
+}
+
+static void approach_limit_switch(bool x, bool y, bool z) {
+  homing_cycle(x, y, z, false, 100000);
+}
+
+static void leave_limit_switch(bool x, bool y, bool z) {
+  homing_cycle(x, y, z, true, 500000);
+}
+
+void limits_go_home() {
+  st_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
+  approach_limit_switch(true, true, false);  // Then home the x and y axis
+  // Xor previous and current limit switch state to determine which were high then but have become 
+  // low now. These are the actual installed limit switches.
+  uint8_t limit_switches_present = (original_limit_state ^ LIMIT_PIN) & LIMIT_MASK;
+  // Now carefully leave the limit switches
+  leave_limit_switch(
+    limit_switches_present & (1<<X_LIMIT_BIT), 
+    limit_switches_present & (1<<Y_LIMIT_BIT),
+    limit_switches_present & (1<<Z_LIMIT_BIT));
+}

limits.h

@@ -1,5 +1,5 @@
 /*
-  serial_protocol.h - the serial protocol master control unit
+  limits.h - code pertaining to limit-switches and performing the homing cycle
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
@@ -17,14 +17,14 @@
   You should have received a copy of the GNU General Public License
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
-#ifndef serial_h
-#define serial_h
 
-// Initialize the serial protocol
+#ifndef limits_h
-void sp_init();
+#define limits_h 
 
-// Read command lines from the serial port and execute them as they
+// initialize the limits module
-// come in. Blocks until the serial buffer is emptied. 
+void limits_init();
-void sp_process();
+
+// perform the homing cycle
+void limits_go_home();
 
 #endif

main.c

@@ -20,33 +20,36 @@
 
 #include <avr/io.h>
 #include <avr/sleep.h>
+#include <avr/interrupt.h>
 #include <util/delay.h>
+#include "config.h"
 #include "planner.h"
 #include "stepper.h"
 #include "spindle_control.h"
 #include "motion_control.h"
 #include "gcode.h"
-#include "serial_protocol.h"
+#include "protocol.h"
+#include "limits.h"
 
 #include "settings.h"
-#include "wiring_serial.h"
+#include "serial.h"
-
-// #ifndef __AVR_ATmega328P__
-// #  error "As of version 0.6 Grbl only supports atmega328p. If you want to run Grbl on an 168 check out 0.51 ('git co v0_51')"
-// #endif
 
 int main(void)
 {
-  sp_init();        
+  sei(); // Enable interrupts
+  
+  serial_init(BAUD_RATE);
+  protocol_init();        
   settings_init();  
   plan_init();      
   st_init();        
   spindle_init();   
   gc_init();
+  limits_init();
                     
-  for(;;){
+  while (1) {
-    sleep_mode(); // Wait for it ...
+//     sleep_mode(); // Wait for it ...
-    sp_process(); // ... process the serial protocol
+    protocol_process(); // ... process the serial protocol
   }
   return 0;   /* never reached */
 }

motion_control.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
@@ -20,6 +21,7 @@
 
 #include <avr/io.h>
 #include "settings.h"
+#include "config.h"
 #include "motion_control.h"
 #include <util/delay.h>
 #include <math.h>
@@ -27,56 +29,115 @@
 #include "nuts_bolts.h"
 #include "stepper.h"
 #include "planner.h"
-#include "wiring_serial.h"
 
-
+// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application.
-void mc_dwell(uint32_t milliseconds) 
+void mc_dwell(double seconds) 
 {
+   uint16_t i = floor(seconds);
    st_synchronize();
-  _delay_ms(milliseconds);
+   _delay_ms(floor(1000*(seconds-i))); // Delay millisecond remainder
+   while (i > 0) {
+     _delay_ms(1000); // Delay one second
+     i--;
+   }
 }
-
-// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
-// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
-// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
-// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
-// position is a pointer to a vector representing the current position in millimeters.
 
 #ifdef __AVR_ATmega328P__
 // 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 theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 
+void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, 
-  int axis_linear, double feed_rate, int invert_feed_rate, double *position)
+  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();
+  double center_axis0 = position[axis_0] + offset[axis_0];
-  plan_set_acceleration_manager_enabled(FALSE); // disable acceleration management for the duration of the arc
+  double center_axis1 = position[axis_1] + offset[axis_1];
-  double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel));
+  double linear_travel = target[axis_linear] - position[axis_linear];
+  double r_axis0 = -offset[axis_0];  // Radius vector from center to current location
+  double r_axis1 = -offset[axis_1];
+  double rt_axis0 = target[axis_0] - center_axis0;
+  double rt_axis1 = target[axis_1] - center_axis1;
+  
+  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
+  double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
+  if (angular_travel < 0) { angular_travel += 2*M_PI; }
+  if (isclockwise) { angular_travel -= 2*M_PI; }
+  
+  double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
   if (millimeters_of_travel == 0.0) { return; }
-  uint16_t segments = ceil(millimeters_of_travel/settings.mm_per_arc_segment);
+  uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment);
   // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
   // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
   // all segments.
   if (invert_feed_rate) { feed_rate *= segments; }
-  // The angular motion for each segment
+ 
   double theta_per_segment = angular_travel/segments;
-  // The linear motion for each segment
   double linear_per_segment = linear_travel/segments;
-  // Compute the center of this circle
+  
-  double center_x = position[axis_1]-sin(theta)*radius;
+  /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
-  double center_y = position[axis_2]-cos(theta)*radius;
+     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
-  // a vector to track the end point of each segment
+         r_T = [cos(phi) -sin(phi);
-  double target[3];
+                sin(phi)  cos(phi] * r ;
-  int i;
+     
+     For arc generation, the center of the circle is the axis of rotation and the radius vector is 
+     defined from the circle center to the initial position. Each line segment is formed by successive
+     vector rotations. This requires only two cos() and sin() computations to form the rotation
+     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
+     all double numbers are single precision on the Arduino. (True double precision will not have
+     round off issues for CNC applications.) Single precision error can accumulate to be greater than
+     tool precision in some cases. Therefore, arc path correction is implemented. 
+
+     Small angle approximation may be used to reduce computation overhead further. This approximation
+     holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
+     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
+     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for 
+     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
+     issue for CNC machines with the single precision Arduino calculations.
+     
+     This approximation also allows mc_arc to immediately insert a line segment into the planner 
+     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
+     a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. 
+     This is important when there are successive arc motions. 
+  */
+  // Vector rotation matrix values
+  double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
+  double sin_T = theta_per_segment;
+  
+  double arc_target[3];
+  double sin_Ti;
+  double cos_Ti;
+  double r_axisi;
+  uint16_t i;
+  int8_t count = 0;
+
   // Initialize the linear axis
-  target[axis_linear] = position[axis_linear];
+  arc_target[axis_linear] = position[axis_linear];
-  for (i=0; i<=segments; i++) {
+
-    target[axis_linear] += linear_per_segment;
+  for (i = 1; i<segments; i++) { // Increment (segments-1)
-    theta += theta_per_segment;
+    
-    target[axis_1] = center_x+sin(theta)*radius;
+    if (count < N_ARC_CORRECTION) {
-    target[axis_2] = center_y+cos(theta)*radius;
+      // Apply vector rotation matrix 
-    plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
+      r_axisi = r_axis0*sin_T + r_axis1*cos_T;
+      r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
+      r_axis1 = r_axisi;
+      count++;
+    } else {
+      // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+      // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+      cos_Ti = cos(i*theta_per_segment);
+      sin_Ti = sin(i*theta_per_segment);
+      r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
+      r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
+      count = 0;
     }
-  plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
+
+    // Update arc_target location
+    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);
+    
+  }
+  // Ensure last segment arrives at target location.
+  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
 }
 #endif
 

motion_control.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
@@ -24,24 +25,27 @@
 #include <avr/io.h>
 #include "planner.h"
 
+// NOTE: Although the following functions structurally belongs in this module, there is nothing to do but
+// to forward the request to the planner.
+
 // 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) 
-// NOTE: Although this function structurally belongs in this module, there is nothing to do but
+
-// to forward the request to the planner. For efficiency the function is implemented with a macro.
+#define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z) 
 
 #ifdef __AVR_ATmega328P__
-// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
+// Execute an arc in offset mode format. position == current xyz, target == target xyz, 
-// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
+// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
-// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
+// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
-// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
+// for vector transformation direction.
-void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 
+void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
-  int axis_linear, double feed_rate, int invert_feed_rate, double *position);
+  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise);
 #endif
   
-// Dwell for a couple of time units
+// Dwell for a specific number of seconds
-void mc_dwell(uint32_t milliseconds);
+void mc_dwell(double seconds);
 
 // Send the tool home (not implemented)
 void mc_go_home();

nuts_bolts.c

@@ -0,0 +1,38 @@
+/*
+  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>
+
+int read_double(char *line, uint8_t *char_counter, double *double_ptr)                  
+{
+  char *start = line + *char_counter;
+  char *end;
+  
+  *double_ptr = strtod(start, &end);
+  if(end == start) { 
+    return(false); 
+  };
+
+  *char_counter = end - line;
+  return(true);
+}
+

nuts_bolts.h

@@ -1,8 +1,9 @@
 /*
-  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
+  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
@@ -21,15 +22,24 @@
 #ifndef nuts_bolts_h
 #define nuts_bolts_h
 #include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
 
-#define FALSE 0
+#define false 0
-#define TRUE 1
+#define true 1
 
 #define X_AXIS 0
 #define Y_AXIS 1
 #define Z_AXIS 2
 
 #define clear_vector(a) memset(a, 0, sizeof(a))
+#define clear_vector_double(a) memset(a, 0.0, sizeof(a))
 #define max(a,b) (((a) > (b)) ? (a) : (b))
+#define min(a,b) (((a) < (b)) ? (a) : (b))
+
+// 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
+int read_double(char *line, uint8_t *char_counter, double *double_ptr);
 
 #endif

planner.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
@@ -20,38 +21,6 @@
 
 /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
 
-/*  
-  Reasoning behind the mathematics in this 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)
-*/
-                                                                                                            
-
 #include <inttypes.h>
 #include <math.h>       
 #include <stdlib.h>
@@ -61,39 +30,49 @@
 #include "stepper.h"
 #include "settings.h"
 #include "config.h"
-#include "wiring_serial.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 int block_buffer_head;           // Index of the next block to be pushed
+static volatile uint8_t block_buffer_head;       // Index of the next block to be pushed
-static volatile int block_buffer_tail;           // Index of the block to process now
+static volatile uint8_t block_buffer_tail;       // Index of the block to process now
 
-// The current position of the tool in absolute steps
+static int32_t position[3];             // The current position of the tool in absolute steps
-static int32_t position[3];   
+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?
 
-#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
+// Returns the index of the next block in the ring buffer
+// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
+static uint8_t next_block_index(uint8_t block_index) 
+{
+  block_index++;
+  if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
+  return(block_index);
+}
+
+
+// Returns the index of the previous block in the ring buffer
+static uint8_t prev_block_index(uint8_t block_index) 
+{
+  if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
+  block_index--;
+  return(block_index);
+}
+
 
 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
 // given acceleration:
-inline 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)/
+  return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) );
-    (2L*acceleration)
-  );
 }
 
-// This function gives you the point at which you must start braking (at the rate of -acceleration) if 
-// 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)
 
 /*                        + <- some maximum rate we don't care about
                          /|\
@@ -104,19 +83,113 @@ inline double estimate_acceleration_distance(double initial_rate, double target_
                           ^  ^                   
                           |  |                   
       intersection_distance  distance                                                                           */
-
+// This function gives you the point at which you must start braking (at the rate of -acceleration) if 
-inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) {
+// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
-  return(
+// a total travel of distance. This can be used to compute the intersection point between acceleration and
-    (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
+// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
-    (4*acceleration)
+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) );
 }
 
             
-// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
+// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity
-// The factors represent a factor of braking and must be in the range 0.0-1.0.
+// using the acceleration within the allotted distance.
+// 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) 
+{
+  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) 
+{
+  if (!current) { return; }  // Cannot operate on nothing.
+  
+  if (next) { 
+    // 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 != current->max_entry_speed) {
+    
+      // If nominal length true, max junction speed is guaranteed to be reached. Only compute
+      // for max allowable speed if block is decelerating and nominal length is false.
+      if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
+        current->entry_speed = min( current->max_entry_speed,
+          max_allowable_speed(-settings.acceleration,next->entry_speed,current->millimeters));
+      } else {
+        current->entry_speed = current->max_entry_speed;
+      } 
+      current->recalculate_flag = true;    
+    
+    }
+  } // Skip last block. Already initialized and set for recalculation.
+}
+
+
+// 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() 
+{
+  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 );
+    block[2]= block[1];
+    block[1]= block[0];
+    block[0] = &block_buffer[block_index];
+    planner_reverse_pass_kernel(block[0], block[1], block[2]);
+  }
+  // Skip buffer tail/first block to prevent over-writing the initial entry speed.
+}
+
+
+// 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) 
+{
+  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
+  // full speed change within the block, we need to adjust the entry speed accordingly. Entry
+  // speeds have already been reset, maximized, and reverse planned by reverse planner.
+  // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.  
+  if (!previous->nominal_length_flag) {
+    if (previous->entry_speed < current->entry_speed) {
+      double entry_speed = min( current->entry_speed,
+        max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) );
+
+      // Check for junction speed change
+      if (current->entry_speed != entry_speed) {
+        current->entry_speed = entry_speed;
+        current->recalculate_flag = true;
+      }
+    }    
+  }
+}
+
+
+// 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() 
+{
+  uint8_t block_index = block_buffer_tail;
+  block_t *block[3] = {NULL, NULL, NULL};
+  
+  while(block_index != block_buffer_head) {
+    block[0] = block[1];
+    block[1] = block[2];
+    block[2] = &block_buffer[block_index];
+    planner_forward_pass_kernel(block[0],block[1],block[2]);
+    block_index = next_block_index( block_index );
+  }
+  planner_forward_pass_kernel(block[1], block[2], NULL);
+}
+
+
+/*                             STEPPER RATE DEFINITION                                              
                                      +--------+   <- nominal_rate
                                     /          \                                
     nominal_rate*entry_factor ->   +            \                               
@@ -124,11 +197,15 @@ inline double intersection_distance(double initial_rate, double final_rate, doub
                                    +-------------+                              
                                        time -->                                 
 */                                                                              
-
+// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
-void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) {
+// The factors represent a factor of braking and must be in the range 0.0-1.0.
-  block->initial_rate = ceil(block->nominal_rate*entry_factor);
+// This converts the planner parameters to the data required by the stepper controller.
-  block->final_rate = ceil(block->nominal_rate*exit_factor);
+// NOTE: Final rates must be computed in terms of their respective blocks.
-  int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0;
+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)
   int32_t accelerate_steps = 
     ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute));
   int32_t decelerate_steps = 
@@ -143,6 +220,8 @@ void calculate_trapezoid_for_block(block_t *block, double entry_factor, double e
   if (plateau_steps < 0) {  
     accelerate_steps = ceil(
       intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count));
+    accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
+    accelerate_steps = min(accelerate_steps,block->step_event_count);
     plateau_steps = 0;
   }  
   
@@ -150,125 +229,22 @@ void calculate_trapezoid_for_block(block_t *block, double entry_factor, double e
   block->decelerate_after = accelerate_steps+plateau_steps;
 }     
 
-// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the 
+/*                            PLANNER SPEED DEFINITION                                              
-// acceleration within the allotted distance.
+                                     +--------+   <- current->nominal_speed
-inline double max_allowable_speed(double acceleration, double target_velocity, double distance) {
+                                    /          \                                
-  return(
+         current->entry_speed ->   +            \                               
-    sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance)
+                                   |             + <- next->entry_speed
-  );
+                                   +-------------+                              
-}
+                                       time -->                                 
-
+*/                                                                              
-// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
+// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the 
-// This method will calculate the junction jerk as the euclidean distance between the nominal 
+// entry_speed for each junction and the entry_speed of the next junction. Must be called by 
-// velocities of the respective blocks.
+// planner_recalculate() after updating the blocks. Any recalulate flagged junction will
-inline double junction_jerk(block_t *before, block_t *after) {
+// compute the two adjacent trapezoids to the junction, since the junction speed corresponds 
-  return(sqrt(
+// to exit speed and entry speed of one another.
-    pow(before->speed_x-after->speed_x, 2)+
+static void planner_recalculate_trapezoids() 
-    pow(before->speed_y-after->speed_y, 2)+
+{
-    pow(before->speed_z-after->speed_z, 2))
+  uint8_t block_index = block_buffer_tail;
-  );
-}
-
-// Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the 
-// speed under which you cannot exceed max_jerk no matter what you do.
-double factor_for_safe_speed(block_t *block) {
-  return(settings.max_jerk/block->nominal_speed);  
-}
-
-// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
-void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
-  if(!current) { return; }
-
-  double entry_factor = 1.0;
-  double exit_factor;
-  if (next) {
-    exit_factor = next->entry_factor;
-  } else {
-    exit_factor = factor_for_safe_speed(current);
-  }
-  
-  // Calculate the entry_factor for the current block. 
-  if (previous) {
-    // Reduce speed so that junction_jerk is within the maximum allowed
-    double jerk = junction_jerk(previous, current);
-    if (jerk > settings.max_jerk) {
-      entry_factor = (settings.max_jerk/jerk);
-    } 
-    // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
-    if (entry_factor > exit_factor) {
-      double max_entry_speed = max_allowable_speed(-settings.acceleration,current->nominal_speed*exit_factor, 
-        current->millimeters);
-      double max_entry_factor = max_entry_speed/current->nominal_speed;
-      if (max_entry_factor < entry_factor) {
-        entry_factor = max_entry_factor;
-      }
-    }    
-  } else {
-    entry_factor = factor_for_safe_speed(current);
-  }
-    
-  // Store result
-  current->entry_factor = entry_factor;
-}
-
-// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
-// implements the reverse pass.
-void planner_reverse_pass() {
-  auto int8_t block_index = block_buffer_head;
-  block_t *block[3] = {NULL, NULL, NULL};
-  while(block_index != block_buffer_tail) {    
-    block_index--;
-    if(block_index < 0) {
-      block_index = BLOCK_BUFFER_SIZE-1;
-    }
-    block[2]= block[1];
-    block[1]= block[0];
-    block[0] = &block_buffer[block_index];
-    planner_reverse_pass_kernel(block[0], block[1], block[2]);
-  }
-  planner_reverse_pass_kernel(NULL, block[0], block[1]);
-}
-
-// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
-void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
-  if(!current) { return; }
-  if(previous) {
-    // If the previous block is an acceleration block, but it is not long enough to 
-    // complete the full speed change within the block, we need to adjust out entry
-    // speed accordingly. Remember current->entry_factor equals the exit factor of 
-    // the previous block.
-    if(previous->entry_factor < current->entry_factor) {
-      double max_entry_speed = max_allowable_speed(-settings.acceleration,
-        current->nominal_speed*previous->entry_factor, previous->millimeters);
-      double max_entry_factor = max_entry_speed/current->nominal_speed;
-      if (max_entry_factor < current->entry_factor) {
-        current->entry_factor = max_entry_factor;
-      }
-    }
-  }
-}
-
-// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
-// implements the forward pass.
-void planner_forward_pass() {
-  int8_t block_index = block_buffer_tail;
-  block_t *block[3] = {NULL, NULL, NULL};
-  
-  while(block_index != block_buffer_head) {
-    block[0] = block[1];
-    block[1] = block[2];
-    block[2] = &block_buffer[block_index];
-    planner_forward_pass_kernel(block[0],block[1],block[2]);
-    block_index = (block_index+1) % BLOCK_BUFFER_SIZE;
-  }
-  planner_forward_pass_kernel(block[1], block[2], NULL);
-}
-
-// Recalculates the trapezoid speed profiles for all blocks in the plan according to the 
-// entry_factor for each junction. Must be called by planner_recalculate() after 
-// updating the blocks.
-void planner_recalculate_trapezoids() {
-  int8_t block_index = block_buffer_tail;
   block_t *current;
   block_t *next = NULL;
   
@@ -276,71 +252,78 @@ void planner_recalculate_trapezoids() {
     current = next;
     next = &block_buffer[block_index];
     if (current) {
-      calculate_trapezoid_for_block(current, current->entry_factor, next->entry_factor);      
+      // Recalculate if current block entry or exit junction speed has changed.
+      if (current->recalculate_flag || next->recalculate_flag) {
+        // NOTE: Entry and exit factors always > 0 by all previous logic operations.     
+        calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
+          next->entry_speed/current->nominal_speed);      
+        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
       }
-    block_index = (block_index+1) % BLOCK_BUFFER_SIZE;
     }
-  calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next));
+    block_index = next_block_index( block_index );
+  }
+  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
+  calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed,
+    MINIMUM_PLANNER_SPEED/next->nominal_speed);
+  next->recalculate_flag = false;
 }
 
 // Recalculates the motion plan according to the following algorithm:
 //
-//   1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor) 
+//   1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) 
 //      so that:
-//     a. The junction jerk is within the set limit
+//     a. The junction speed is equal to or less than the maximum junction speed limit
 //     b. No speed reduction within one block requires faster deceleration than the one, true constant 
 //        acceleration.
-//   2. Go over every block in chronological order and dial down junction speed reduction values if 
+//   2. Go over every block in chronological order and dial down junction speed values if 
-//     a. The speed increase within one block would require faster accelleration than the one, true 
+//     a. The speed increase within one block would require faster acceleration than the one, true 
 //        constant acceleration.
 //
-// When these stages are complete all blocks have an entry_factor that will allow all speed changes to 
+// When these stages are complete all blocks have an entry speed that will allow all speed changes to 
-// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than 
+// be performed using only the one, true constant acceleration, and where no junction speed is greater
-// the set limit. Finally it will:
+// than the max limit. Finally it will:
 //
-//   3. Recalculate trapezoids for all blocks.
+//   3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids
+//      with no updated junction speeds will not be recalculated and assumed ok as is.
+//
+// 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.
 
-void planner_recalculate() {     
+static void planner_recalculate() 
+{     
   planner_reverse_pass();
   planner_forward_pass();
   planner_recalculate_trapezoids();
 }
 
-void plan_init() {
+void plan_init() 
+{
   block_buffer_head = 0;
   block_buffer_tail = 0;
-  plan_set_acceleration_manager_enabled(TRUE);
   clear_vector(position);
+  clear_vector_double(previous_unit_vec);
+  previous_nominal_speed = 0.0;
 }
 
-void plan_set_acceleration_manager_enabled(int 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);
-}
-
-inline void plan_discard_current_block() {
   if (block_buffer_head != block_buffer_tail) {
-    block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE;  
+    block_buffer_tail = next_block_index( block_buffer_tail );
   }
 }
 
-inline 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]);
 }
 
-// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
-// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
-// calculation the caller must also provide the physical length of the line in millimeters.
-void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) {
-  // The target position of the tool in absolute steps
 
+// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
+// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
+// 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) 
+{  
   // Calculate target position in absolute steps
   int32_t target[3];
   target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
@@ -348,63 +331,13 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
   target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);     
   
   // Calculate the buffer head after we push this byte
-	int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE;	
+  uint8_t 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];
-  // Number of steps for each axis
-  block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
-  block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
-  block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
-  block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
-  // Bail if this is a zero-length block
-  if (block->step_event_count == 0) { return; };
-  
-  double delta_x_mm = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS];
-  double delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
-  double delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
-  block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm));
-	
-  
-  uint32_t microseconds;
-  if (!invert_feed_rate) {
-    microseconds = lround((block->millimeters/feed_rate)*1000000);
-  } else {
-    microseconds = lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate);
-  }
-  
-  // Calculate speed in mm/minute for each axis
-  double multiplier = 60.0*1000000.0/microseconds;
-  block->speed_x = delta_x_mm * multiplier;
-  block->speed_y = delta_y_mm * multiplier;
-  block->speed_z = delta_z_mm * multiplier; 
-  block->nominal_speed = block->millimeters * multiplier;
-  block->nominal_rate = ceil(block->step_event_count * multiplier);  
-  block->entry_factor = 0.0;
-  
-  // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
-  // average travel per step event changes. For a line along one axis the travel per step event
-  // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
-  // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
-  // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed 
-  // specifically for each line to compensate for this phenomenon:
-  double travel_per_step = block->millimeters/block->step_event_count;
-  block->rate_delta = ceil(
-    ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick
-    travel_per_step);                                               // convert to: acceleration steps/min/acceleration_tick    
-  if (acceleration_manager_enabled) {
-    // compute a preliminary conservative acceleration trapezoid
-    double safe_speed_factor = factor_for_safe_speed(block);
-    calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor); 
-  } else {
-    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;
-  }
 
   // Compute direction bits for this block
   block->direction_bits = 0;
@@ -412,12 +345,119 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
   if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
   if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
   
+  // Number of steps for each axis
+  block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
+  block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
+  block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
+  block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
+
+  // Bail if this is a zero-length block
+  if (block->step_event_count == 0) { return; };
+  
+  // Compute path vector in terms of absolute step target and current positions
+  double delta_mm[3];
+  delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS];
+  delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
+  delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
+  block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + 
+                            square(delta_mm[Z_AXIS]));
+  double inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides	
+  
+  // Calculate speed in mm/minute 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
+  double inverse_minute;
+  if (!invert_feed_rate) {
+    inverse_minute = feed_rate * inverse_millimeters;
+  } else {
+    inverse_minute = 1.0 / feed_rate;
+  }
+  block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
+  block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0
+  
+  // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
+  // average travel per step event changes. For a line along one axis the travel per step event
+  // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
+  // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
+  // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed 
+  // specifically for each line to compensate for this phenomenon:
+  // Convert universal acceleration for direction-dependent stepper rate change parameter
+  block->rate_delta = ceil( block->step_event_count*inverse_millimeters *  
+        settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick)
+
+  // Compute path unit vector                            
+  double unit_vec[3];
+
+  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;  
+
+  // 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.
+  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
+    // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
+    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
+    double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
+                       - 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;
+  
   // 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_wake_up();
+  st_cycle_start();
 }
 
+// Reset the planner position vector and planner speed
+void plan_set_current_position(double x, double y, double z) {
+  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]);    
+  previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
+  clear_vector_double(previous_unit_vec);
+}

planner.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
@@ -18,9 +19,6 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-// This module is to be considered a sub-module of stepper.c. Please don't include 
-// this file from any other module.
-
 #ifndef planner_h
 #define planner_h
                  
@@ -29,26 +27,27 @@
 // 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 speed_x, speed_y, speed_z;   // Nominal mm/minute for each axis
   double nominal_speed;               // The nominal speed for this block in mm/min  
+  double entry_speed;                 // Entry speed at previous-current junction in mm/min
+  double max_entry_speed;             // Maximum allowable junction entry speed in mm/min
   double millimeters;                 // The total travel of this block in mm
-  double entry_factor;                // The factor representing the change in speed at the start of this trapezoid.
+  uint8_t recalculate_flag;           // Planner flag to recalculate trapezoids on entry junction
-                                      // (The end of the curren speed trapezoid is defined by the entry_factor of the
+  uint8_t nominal_length_flag;        // Planner flag for nominal speed always reached
-                                      // next block)
 
   // 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;
       
@@ -58,19 +57,16 @@ void plan_init();
 // 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
 // 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, int invert_feed_rate);
+void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate);
 
 // Called when the current block is no longer needed. Discards the block and makes the memory
 // availible for new blocks.
-inline void plan_discard_current_block();
+void plan_discard_current_block();
 
 // Gets the current block. Returns NULL if buffer empty
-inline block_t *plan_get_current_block();
+block_t *plan_get_current_block();
 
-// Enables or disables acceleration-management for upcoming blocks
+// Reset the position vector
-void plan_set_acceleration_manager_enabled(int enabled);
+void plan_set_current_position(double x, double y, double z); 
-
-// Is acceleration-management currently enabled?
-int plan_is_acceleration_manager_enabled();
 
 #endif

print.c

@@ -0,0 +1,104 @@
+/*
+  print.c - Functions for formatting output strings
+  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
+  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/>.
+*/
+
+/* This code was initially inspired by the wiring_serial module by David A. Mellis which
+   used to be a part of the Arduino project. */ 
+
+
+#include <math.h>
+#include <avr/pgmspace.h>
+#include "serial.h"
+
+#ifndef DECIMAL_PLACES
+#define DECIMAL_PLACES 3
+#define DECIMAL_MULTIPLIER 10*10*10
+#endif
+
+void printString(const char *s)
+{
+  while (*s)
+    serial_write(*s++);
+}
+
+// Print a string stored in PGM-memory
+void printPgmString(const char *s)
+{
+  char c;
+  while ((c = pgm_read_byte_near(s++)))
+    serial_write(c);
+}
+
+void printIntegerInBase(unsigned long n, unsigned long base)
+{ 
+	unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. 
+	unsigned long i = 0;
+
+	if (n == 0) {
+		serial_write('0');
+		return;
+	} 
+
+	while (n > 0) {
+		buf[i++] = n % base;
+		n /= base;
+	}
+
+	for (; i > 0; i--)
+		serial_write(buf[i - 1] < 10 ?
+			'0' + buf[i - 1] :
+			'A' + buf[i - 1] - 10);
+}
+
+void printInteger(long n)
+{
+	if (n < 0) {
+		serial_write('-');
+		n = -n;
+	}
+
+	printIntegerInBase(n, 10);
+}
+
+// A very simple 
+void printFloat(double n)
+{
+  if (n < 0) {
+    serial_write('-');
+    n = -n;
+  }
+  n += 0.5/DECIMAL_MULTIPLIER; // Add rounding factor
+ 
+  long integer_part;
+  integer_part = (int)n;
+  printIntegerInBase(integer_part,10);
+  
+  serial_write('.');
+  
+  n -= integer_part;
+  int decimals = DECIMAL_PLACES;  
+  uint8_t decimal_part;  
+  while(decimals-- > 0) {
+    n *= 10;
+    decimal_part = (int) n;
+    serial_write('0'+decimal_part);
+    n -= decimal_part;
+  }
+}

print.h

@@ -0,0 +1,37 @@
+/*
+  print.h - Functions for formatting output strings
+  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/>.
+*/
+
+/* This code was initially inspired by the wiring_serial module by David A. Mellis which
+   used to be a part of the Arduino project. */ 
+
+#ifndef print_h
+#define print_h
+
+void printString(const char *s);
+
+void printPgmString(const char *s);
+
+void printInteger(long n);
+
+void printIntegerInBase(unsigned long n, unsigned long base);
+
+void printFloat(double n);
+
+#endif

protocol.c

@@ -0,0 +1,122 @@
+/*
+  protocol.c - the serial protocol master control unit
+  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
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <avr/io.h>
+#include "protocol.h"
+#include "gcode.h"
+#include "serial.h"
+#include "print.h"
+#include "settings.h"
+#include "config.h"
+#include <math.h>
+#include "nuts_bolts.h"
+#include <avr/pgmspace.h>
+#define LINE_BUFFER_SIZE 50
+
+static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated.
+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) 
+{
+  if (status_code == 0) {
+    printPgmString(PSTR("ok\r\n"));
+  } else {
+    printPgmString(PSTR("error: "));
+    switch(status_code) {          
+      case STATUS_BAD_NUMBER_FORMAT:
+      printPgmString(PSTR("Bad number format\r\n")); break;
+      case STATUS_EXPECTED_COMMAND_LETTER:
+      printPgmString(PSTR("Expected command letter\r\n")); break;
+      case STATUS_UNSUPPORTED_STATEMENT:
+      printPgmString(PSTR("Unsupported statement\r\n")); break;
+      case STATUS_FLOATING_POINT_ERROR:
+      printPgmString(PSTR("Floating point error\r\n")); break;
+      default:
+      printInteger(status_code);
+      printPgmString(PSTR("\r\n"));
+    }
+  }
+}
+
+void protocol_init() 
+{
+  char_counter = 0; // Reset line input
+  iscomment = false;
+  printPgmString(PSTR("\r\nGrbl " GRBL_VERSION));
+  printPgmString(PSTR("\r\n"));  
+}
+
+// Executes one line of input according to protocol
+uint8_t protocol_execute_line(char *line) 
+{     
+  if(line[0] == '$') {
+    return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module
+  } 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;
+  while((c = serial_read()) != SERIAL_NO_DATA) 
+  {
+    if ((c == '\n') || (c == '\r')) { // End of line reached
+      if (char_counter > 0) {// Line is complete. Then execute!
+        line[char_counter] = 0; // Terminate string
+        status_message(protocol_execute_line(line));
+      } else { 
+        // Empty or comment line. Skip block.
+        status_message(STATUS_OK); // Send status message for syncing purposes.
+      }
+      char_counter = 0; // Reset line buffer index
+      iscomment = false; // Reset comment flag
+    } else {
+      if (iscomment) {
+        // Throw away all comment characters
+        if (c == ')') {
+          // End of comment. Resume line.
+          iscomment = false;
+        }
+      } else {
+        if (c <= ' ') { 
+          // Throw away whitepace and control characters
+        } else if (c == '/') {
+          // Disable block delete and throw away character
+          // To enable block delete, uncomment following line. Will ignore until EOL.
+          // iscomment = true;
+        } else if (c == '(') {
+          // Enable comments flag and ignore all characters until ')' or EOL.
+          iscomment = true;
+        } else if (char_counter >= LINE_BUFFER_SIZE-1) {
+          // Throw away any characters beyond the end of the line buffer
+        } else if (c >= 'a' && c <= 'z') { // Upcase lowercase
+          line[char_counter++] = c-'a'+'A';
+        } else {
+          line[char_counter++] = c;
+        }
+      }
+    }
+  }
+}

protocol.h

@@ -0,0 +1,40 @@
+/*
+  protocol.h - the serial protocol master control unit
+  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
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#ifndef protocol_h
+#define protocol_h
+
+#define STATUS_OK 0
+#define STATUS_BAD_NUMBER_FORMAT 1
+#define STATUS_EXPECTED_COMMAND_LETTER 2
+#define STATUS_UNSUPPORTED_STATEMENT 3
+#define STATUS_FLOATING_POINT_ERROR 4
+
+// Initialize the serial protocol
+void protocol_init();
+
+// Read command lines from the serial port and execute them as they
+// come in. Blocks until the serial buffer is emptied. 
+void protocol_process();
+
+// Executes one line of input according to protocol
+uint8_t protocol_execute_line(char *line);
+
+#endif

readme.textile

@@ -4,12 +4,20 @@ 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.
+Grbl includes full acceleration management with look ahead. That means the controller will look up to 16 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. This had to go, as I was unable to fit the acceleration management into the 16k code space of the 168. If you want to run Grbl on an 168 I am still maintaining Grbl 0.51 "in the branch called 'v0_51'":https://github.com/simen/grbl/tree/v0_51.
+*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.
 
-*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.
+*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_

script/simple_stream.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+"""\
+Simple g-code streaming script for grbl
+"""
+
+import serial
+import time
+
+# Open grbl serial port
+s = serial.Serial('/dev/tty.usbmodem1811',9600)
+
+# Open g-code file
+f = open('grbl.gcode','r');
+
+# Wake up grbl
+s.write("\r\n\r\n")
+time.sleep(2)   # Wait for grbl to initialize 
+s.flushInput()  # Flush startup text in serial input
+
+# Stream g-code to grbl
+for line in f:
+    l = line.strip() # Strip all EOL characters for consistency
+    print 'Sending: ' + l,
+    s.write(l + '\n') # Send g-code block to grbl
+    grbl_out = s.readline() # Wait for grbl response with carriage return
+    print ' : ' + grbl_out.strip()
+
+# Wait here until grbl is finished to close serial port and file.
+raw_input("  Press <Enter> to exit and disable grbl.") 
+
+# Close file and serial port
+f.close()
+s.close()    

serial.c

@@ -0,0 +1,125 @@
+/*
+  serial.c - Low level functions for sending and recieving bytes via the serial port
+  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/>.
+*/
+
+/* This code was initially inspired by the wiring_serial module by David A. Mellis which
+   used to be a part of the Arduino project. */ 
+
+#include <avr/interrupt.h>
+#include <avr/sleep.h>
+#include "serial.h"
+
+
+#ifdef __AVR_ATmega328P__
+#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;
+uint8_t rx_buffer_tail = 0;
+
+uint8_t tx_buffer[TX_BUFFER_SIZE];
+uint8_t tx_buffer_head = 0;
+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;
+  UBRR0L = UBRR0_value;
+}
+
+void serial_init(long baud)
+{
+  set_baud_rate(baud);
+  
+  /* baud doubler off  - Only needed on Uno XXX */
+  UCSR0A &= ~(1 << U2X0);
+          
+  // enable rx and tx
+  UCSR0B |= 1<<RXEN0;
+  UCSR0B |= 1<<TXEN0;
+	
+  // enable interrupt on complete reception of a byte
+  UCSR0B |= 1<<RXCIE0;
+	  
+  // defaults to 8-bit, no parity, 1 stop bit
+}
+
+void serial_write(uint8_t data) {
+  // Calculate next head
+  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
+  while (next_head == tx_buffer_tail) { };//sleep_mode(); };
+
+  // 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); 
+}
+
+// Data Register Empty Interrupt handler
+ISR(USART_UDRE_vect) {  
+  // Temporary tx_buffer_tail (to optimize for volatile)
+  uint8_t tail = tx_buffer_tail;
+
+  // Send a byte from the buffer	
+  UDR0 = tx_buffer[tail];
+
+  // Update tail position
+  tail ++;
+  if (tail == TX_BUFFER_SIZE) { tail = 0; }
+
+  // Turn off Data Register Empty Interrupt to stop tx-streaming if this concludes the transfer
+  if (tail == tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); }
+
+  tx_buffer_tail = tail;
+}
+
+uint8_t serial_read()
+{
+  if (rx_buffer_head == rx_buffer_tail) {
+    return SERIAL_NO_DATA;
+  } else {
+    uint8_t data = rx_buffer[rx_buffer_tail];
+    rx_buffer_tail++;
+    if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; }
+    return data;
+  }
+}
+
+ISR(USART_RX_vect)
+{
+  uint8_t data = UDR0;
+  uint8_t next_head = rx_buffer_head + 1;
+  if (next_head == RX_BUFFER_SIZE) { next_head = 0; }
+
+  // Write data to buffer unless it is full.
+  if (next_head != rx_buffer_tail) {
+    rx_buffer[rx_buffer_head] = data;
+    rx_buffer_head = next_head;
+  }
+}

serial.h

@@ -0,0 +1,35 @@
+/*
+  serial.c - Low level functions for sending and recieving bytes via the serial port
+  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/>.
+*/
+
+/* This code was initially inspired by the wiring_serial module by David A. Mellis which
+   used to be a part of the Arduino project. */ 
+
+#ifndef serial_h
+#define serial_h
+
+#define SERIAL_NO_DATA 0xff
+
+void serial_init(long baud);
+
+void serial_write(uint8_t data);
+
+uint8_t serial_read();
+
+#endif

serial_protocol.c

@@ -1,77 +0,0 @@
-/*
-  serial_protocol.c - the serial protocol master control unit
-  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 <avr/io.h>
-#include "serial_protocol.h"
-#include "gcode.h"
-#include "wiring_serial.h"
-#include "settings.h"
-#include "config.h"
-#include <math.h>
-#include "nuts_bolts.h"
-#include <avr/pgmspace.h>
-#define LINE_BUFFER_SIZE 50
-
-static char line[LINE_BUFFER_SIZE];
-static uint8_t char_counter;
-
-void status_message(int status_code) {
-  switch(status_code) {          
-    case GCSTATUS_OK:
-    printPgmString(PSTR("ok\n\r")); break;
-    case GCSTATUS_BAD_NUMBER_FORMAT:
-    printPgmString(PSTR("error: Bad number format\n\r")); break;
-    case GCSTATUS_EXPECTED_COMMAND_LETTER:
-    printPgmString(PSTR("error: Expected command letter\n\r")); break;
-    case GCSTATUS_UNSUPPORTED_STATEMENT:
-    printPgmString(PSTR("error: Unsupported statement\n\r")); break;
-    case GCSTATUS_FLOATING_POINT_ERROR:
-    printPgmString(PSTR("error: Floating point error\n\r")); break;
-    default:
-    printPgmString(PSTR("error: "));
-    printInteger(status_code);
-    printPgmString(PSTR("\n\r"));
-  }
-}
-
-void sp_init() 
-{
-  beginSerial(BAUD_RATE);  
-  printPgmString(PSTR("\r\nGrbl " GRBL_VERSION));
-  printPgmString(PSTR("\r\n"));  
-}
-
-void sp_process()
-{
-  char c;
-  while((c = serialRead()) != -1) 
-  {
-    if((char_counter > 0) && ((c == '\n') || (c == '\r'))) {  // Line is complete. Then execute!
-      line[char_counter] = 0; // treminate string
-      status_message(gc_execute_line(line));
-      char_counter = 0; // reset line buffer index
-    } else if (c <= ' ') { // Throw away whitepace and control characters
-    } else if (c >= 'a' && c <= 'z') { // Upcase lowercase
-      line[char_counter++] = c-'a'+'A';
-    } else {
-      line[char_counter++] = c;
-    }
-  }
-}

settings.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
@@ -23,8 +24,10 @@
 #include "nuts_bolts.h"
 #include "settings.h"
 #include "eeprom.h"
-#include "wiring_serial.h"
+#include "print.h"
 #include <avr/pgmspace.h>
+#include "protocol.h"
+#include "config.h"
 
 settings_t settings;
 
@@ -39,6 +42,19 @@ typedef struct {
   double mm_per_arc_segment;
 } settings_v1_t;
 
+// Default settings (used when resetting eeprom-settings)
+#define MICROSTEPS 8
+#define DEFAULT_X_STEPS_PER_MM (94.488188976378*MICROSTEPS)
+#define DEFAULT_Y_STEPS_PER_MM (94.488188976378*MICROSTEPS)
+#define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS)
+#define DEFAULT_STEP_PULSE_MICROSECONDS 30
+#define DEFAULT_MM_PER_ARC_SEGMENT 0.1
+#define DEFAULT_RAPID_FEEDRATE 500.0 // mm/min
+#define DEFAULT_FEEDRATE 500.0
+#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE*60*60/10.0) // mm/min^2
+#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm
+#define DEFAULT_STEPPING_INVERT_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT))
+
 void settings_reset() {
   settings.steps_per_mm[X_AXIS] = DEFAULT_X_STEPS_PER_MM;
   settings.steps_per_mm[Y_AXIS] = DEFAULT_Y_STEPS_PER_MM;
@@ -49,7 +65,7 @@ void settings_reset() {
   settings.acceleration = DEFAULT_ACCELERATION;
   settings.mm_per_arc_segment = DEFAULT_MM_PER_ARC_SEGMENT;
   settings.invert_mask = DEFAULT_STEPPING_INVERT_MASK;
-  settings.max_jerk = DEFAULT_MAX_JERK;
+  settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION;
 }
 
 void settings_dump() {
@@ -62,12 +78,38 @@ void settings_dump() {
   printPgmString(PSTR(" (mm/min default seek rate)\r\n$6 = ")); printFloat(settings.mm_per_arc_segment);
   printPgmString(PSTR(" (mm/arc segment)\r\n$7 = ")); printInteger(settings.invert_mask); 
   printPgmString(PSTR(" (step port invert mask. binary = ")); printIntegerInBase(settings.invert_mask, 2);  
-  printPgmString(PSTR(")\r\n$8 = ")); printFloat(settings.acceleration);
+  printPgmString(PSTR(")\r\n$8 = ")); printFloat(settings.acceleration/(60*60)); // Convert from mm/min^2 for human readability
-  printPgmString(PSTR(" (acceleration in mm/sec^2)\r\n$9 = ")); printFloat(settings.max_jerk);
+  printPgmString(PSTR(" (acceleration in mm/sec^2)\r\n$9 = ")); printFloat(settings.junction_deviation);
-  printPgmString(PSTR(" (max instant cornering speed change in delta mm/min)"));
+  printPgmString(PSTR(" (cornering junction deviation in mm)"));
   printPgmString(PSTR("\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n"));
 }
 
+// Parameter lines are on the form '$4=374.3' or '$' to dump current settings
+uint8_t settings_execute_line(char *line) {
+  uint8_t char_counter = 1;
+  double parameter, value;
+  if(line[0] != '$') { 
+    return(STATUS_UNSUPPORTED_STATEMENT); 
+  }
+  if(line[char_counter] == 0) { 
+    settings_dump(); return(STATUS_OK); 
+  }
+  if(!read_double(line, &char_counter, &parameter)) {
+    return(STATUS_BAD_NUMBER_FORMAT);
+  };
+  if(line[char_counter++] != '=') { 
+    return(STATUS_UNSUPPORTED_STATEMENT); 
+  }
+  if(!read_double(line, &char_counter, &value)) {
+    return(STATUS_BAD_NUMBER_FORMAT);
+  }
+  if(line[char_counter] != 0) { 
+    return(STATUS_UNSUPPORTED_STATEMENT); 
+  }
+  settings_store_setting(parameter, value);
+  return(STATUS_OK);
+}
+
 void write_settings() {
   eeprom_put_char(0, SETTINGS_VERSION);
   memcpy_to_eeprom_with_checksum(1, (char*)&settings, sizeof(settings_t));
@@ -80,33 +122,51 @@ int read_settings() {
   if (version == SETTINGS_VERSION) {
     // Read settings-record and check checksum
     if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_t)))) {
-      return(FALSE);
+      return(false);
     }
   } else if (version == 1) {
-    // Migrate from old settings version
+    // Migrate from settings version 1
     if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_v1_t)))) {
-      return(FALSE);
+      return(false);
     }
     settings.acceleration = DEFAULT_ACCELERATION;
-    settings.max_jerk = DEFAULT_MAX_JERK;
+    settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION;
-  } else {      
+    write_settings();
-    return(FALSE);
+  } else if ((version == 2) || (version == 3)) {
+    // Migrate from settings version 2 and 3
+    if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_t)))) {
+      return(false);
     }
-  return(TRUE);
+    if (version == 2) { settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; }    
+    settings.acceleration *= 3600; // Convert to mm/min^2 from mm/sec^2
+    write_settings();
+  } else {      
+    return(false);
+  }
+  return(true);
 }
 
 // A helper method to set settings from command line
 void settings_store_setting(int parameter, double value) {
   switch(parameter) {
     case 0: case 1: case 2:
+    if (value <= 0.0) {
+      printPgmString(PSTR("Steps/mm must be > 0.0\r\n"));
+      return;
+    }
     settings.steps_per_mm[parameter] = value; break;
-    case 3: settings.pulse_microseconds = round(value); break;
+    case 3: 
+    if (value < 3) {
+      printPgmString(PSTR("Step pulse must be >= 3 microseconds\r\n"));
+      return;
+    }
+    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.mm_per_arc_segment = value; break;
     case 7: settings.invert_mask = trunc(value); break;
-    case 8: settings.acceleration = value; break;
+    case 8: settings.acceleration = value*60*60; break; // Convert to mm/min^2 for grbl internal use.
-    case 9: settings.max_jerk = fabs(value); break;
+    case 9: settings.junction_deviation = fabs(value); break;
     default: 
       printPgmString(PSTR("Unknown parameter\r\n"));
       return;

settings.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
@@ -25,11 +26,11 @@
 #include <math.h>
 #include <inttypes.h>
 
-#define GRBL_VERSION "0.6b"
+#define GRBL_VERSION "0.7d"
 
 // 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 2
+#define SETTINGS_VERSION 4
 
 // Current global settings (persisted in EEPROM from byte 1 onwards)
 typedef struct {
@@ -41,7 +42,7 @@ typedef struct {
   uint8_t invert_mask;
   double mm_per_arc_segment;
   double acceleration;
-  double max_jerk;
+  double junction_deviation;
 } settings_t;
 extern settings_t settings;
 
@@ -51,20 +52,10 @@ void settings_init();
 // Print current settings
 void settings_dump();
 
+// Handle settings command
+uint8_t settings_execute_line(char *line);
+
 // A helper method to set new settings from command line
 void settings_store_setting(int parameter, double value);
 
-// Default settings (used when resetting eeprom-settings)
-#define MICROSTEPS 8
-#define DEFAULT_X_STEPS_PER_MM (94.488188976378*MICROSTEPS)
-#define DEFAULT_Y_STEPS_PER_MM (94.488188976378*MICROSTEPS)
-#define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS)
-#define DEFAULT_STEP_PULSE_MICROSECONDS 30
-#define DEFAULT_MM_PER_ARC_SEGMENT 0.1
-#define DEFAULT_RAPID_FEEDRATE 480.0 // in millimeters per minute
-#define DEFAULT_FEEDRATE 480.0
-#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/100.0)
-#define DEFAULT_MAX_JERK 50.0
-#define DEFAULT_STEPPING_INVERT_MASK 0
-
 #endif

spindle_control.c

@@ -21,25 +21,36 @@
 #include "spindle_control.h"
 #include "settings.h"
 #include "config.h"
+#include "stepper.h"
 
 #include <avr/io.h>
 
+static int current_direction;
+
 void spindle_init()
 {
-  SPINDLE_ENABLE_DDR |= 1<<SPINDLE_ENABLE_BIT;
+  spindle_run(0, 0);
 }
 
 void spindle_run(int direction, uint32_t rpm) 
 {
-  if(direction >= 0) {
+  if (direction != current_direction) {
+    st_synchronize();
+    if(direction) {
+      if(direction > 0) {
         SPINDLE_DIRECTION_PORT &= ~(1<<SPINDLE_DIRECTION_BIT);
       } else {
         SPINDLE_DIRECTION_PORT |= 1<<SPINDLE_DIRECTION_BIT;
       }
       SPINDLE_ENABLE_PORT |= 1<<SPINDLE_ENABLE_BIT;
+    } else {
+      SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);      
+    }
+    current_direction = direction;
+  }
 }
 
 void spindle_stop()
 {
-  SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
+  spindle_run(0, 0);
 }

stepper.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
@@ -30,23 +31,16 @@
 #include "nuts_bolts.h"
 #include <avr/interrupt.h>
 #include "planner.h"
-#include "wiring_serial.h"
+#include "limits.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
 #define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
-#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
 
 #define TICKS_PER_MICROSECOND (F_CPU/1000000)
 #define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
 
-#define MINIMUM_STEPS_PER_MINUTE 1200 // The stepper subsystem will never run slower than this, exept when sleeping
-
-#define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)
-#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
-
 static block_t *current_block;  // A pointer to the block currently being traced
 
 // Variables used by The Stepper Driver Interrupt
@@ -55,13 +49,15 @@ static int32_t counter_x,       // Counter variables for the bresenham line trac
                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.
+static volatile uint8_t 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.
 
 //         __________________________
 //        /|                        |\     _________________         ^
@@ -77,80 +73,88 @@ static uint32_t trapezoid_adjusted_rate;      // The current rate of step_events
 //  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
 //  during the first block->accelerate_until step_events_completed, then keeps going at constant speed until 
 //  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
-//  The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate by trapezoid_generator_tick()
+//  The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate following the midpoint rule
-//  that is called ACCELERATION_TICKS_PER_SECOND times per second.
+//  by the trapezoid generator, which is called ACCELERATION_TICKS_PER_SECOND times per second.
 
-void set_step_events_per_minute(uint32_t steps_per_minute);
+static void set_step_events_per_minute(uint32_t steps_per_minute);
 
-void st_wake_up() {
+// Stepper state initialization
-  ENABLE_STEPPER_DRIVER_INTERRUPT();  
+static void st_wake_up() 
+{
+  // Initialize stepper output bits
+  out_bits = (0) ^ (settings.invert_mask); 
+  // Enable steppers by resetting the stepper disable port
+  STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
+  // Enable stepper driver interrupt
+  TIMSK1 |= (1<<OCIE1A);
+}
+
+// Stepper shutdown
+static 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
+  // stop and not drift from residual inertial forces at the end of the last movement.
+  #if STEPPER_IDLE_LOCK_TIME
+    _delay_ms(STEPPER_IDLE_LOCK_TIME);   
+  #endif
+  // Disable steppers by setting stepper disable
+  STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
 }
 
 // Initializes the trapezoid generator from the current block. Called whenever a new 
 // block begins.
-inline void trapezoid_generator_reset() {
+static void trapezoid_generator_reset() 
+{
   trapezoid_adjusted_rate = current_block->initial_rate;
-  trapezoid_tick_cycle_counter = 0; // Always start a new trapezoid with a full acceleration tick
+  min_safe_rate = current_block->rate_delta + (current_block->rate_delta >> 1); // 1.5 x rate_delta
-  set_step_events_per_minute(trapezoid_adjusted_rate);
+  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 is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
+// This function determines an acceleration velocity change every CYCLES_PER_ACCELERATION_TICK by
-// interrupt. It can be assumed that the trapezoid-generator-parameters and the
+// keeping track of the number of elapsed cycles during a de/ac-celeration. The code assumes that 
-// current_block stays untouched by outside handlers for the duration of this function call.
+// step_events occur significantly more often than the acceleration velocity iterations.
-inline void trapezoid_generator_tick() {     
+static uint8_t iterate_trapezoid_cycle_counter() 
-  if (current_block) {
+{
-    if (step_events_completed < current_block->accelerate_until) {
+  trapezoid_tick_cycle_counter += cycles_per_step_event;  
-      trapezoid_adjusted_rate += current_block->rate_delta;
+  if(trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) {
-      if (trapezoid_adjusted_rate > current_block->nominal_rate ) {
+    trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK;
-        trapezoid_adjusted_rate = current_block->nominal_rate;
+    return(true);
-      }
-      set_step_events_per_minute(trapezoid_adjusted_rate);
-    } else if (step_events_completed > current_block->decelerate_after) {
-      // NOTE: We will only reduce speed if the result will be > 0. This catches small
-      // rounding errors that might leave steps hanging after the last trapezoid tick.
-      if (trapezoid_adjusted_rate > current_block->rate_delta) {
-        trapezoid_adjusted_rate -= current_block->rate_delta;
-      }
-      if (trapezoid_adjusted_rate < current_block->final_rate) {
-        trapezoid_adjusted_rate = current_block->final_rate;
-      }        
-      set_step_events_per_minute(trapezoid_adjusted_rate);
   } else {
-      // Make sure we cruise at exactly nominal rate
+    return(false);
-      if (trapezoid_adjusted_rate != current_block->nominal_rate) {
-        trapezoid_adjusted_rate = current_block->nominal_rate;
-        set_step_events_per_minute(trapezoid_adjusted_rate);
-      }
-    }
   }
 }          
 
 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with
 // 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. 
-SIGNAL(TIMER1_COMPA_vect)
+// The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts.
+ISR(TIMER1_COMPA_vect)
 {        
-  // TODO: Check if the busy-flag can be eliminated by just disabeling this interrupt while we are in it
-  
   if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
-  // Set the direction pins a cuple of nanoseconds before we step the steppers
+  
+  // 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
+  // Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
-  // exactly settings.pulse_microseconds microseconds.  Clear the overflow flag to stop a queued
+  // exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler.
-  // interrupt from resetting the step pulse too soon.
+  TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Reload timer counter
-  TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8);
+  TCCR2B = (1<<CS21); // Begin timer2. Full speed, 1/8 prescaler
-  TIFR2 |= (1<<TOV2);
 
-  busy = TRUE;
+  busy = true;
-  sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
+  // Re-enable interrupts to allow ISR_TIMER2_OVERFLOW to trigger on-time and allow serial communications
-         // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered 
+  // regardless of time in this handler. The following code prepares the stepper driver for the next
-         // at exactly the right time even if we occasionally spend a lot of time inside this handler.))
+  // step interrupt compare and will always finish before returning to the main program.
+  sei();
   
   // 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();
@@ -159,11 +163,12 @@ SIGNAL(TIMER1_COMPA_vect)
       counter_z = counter_x;
       step_events_completed = 0;     
     } else {
-      DISABLE_STEPPER_DRIVER_INTERRUPT();
+      st_go_idle();
     }    
   } 
 
   if (current_block != NULL) {
+    // Execute step displacement profile by bresenham line algorithm
     out_bits = current_block->direction_bits;
     counter_x += current_block->steps_x;
     if (counter_x > 0) {
@@ -180,35 +185,84 @@ SIGNAL(TIMER1_COMPA_vect)
       out_bits |= (1<<Z_STEP_BIT);
       counter_z -= current_block->step_event_count;
     }
+    
+    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) {
+      // The trapezoid generator always checks step event location to ensure de/ac-celerations are 
+      // executed and terminated at exactly the right time. This helps prevent over/under-shooting
+      // the target position and speed. 
+      // 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 (step_events_completed < current_block->accelerate_until) {
+        // Iterate cycle counter and check if speeds need to be increased.
+        if ( iterate_trapezoid_cycle_counter() ) {
+          trapezoid_adjusted_rate += current_block->rate_delta;
+          if (trapezoid_adjusted_rate >= current_block->nominal_rate) {
+            // Reached nominal rate a little early. Cruise at nominal rate until decelerate_after.
+            trapezoid_adjusted_rate = current_block->nominal_rate;
+          }
+          set_step_events_per_minute(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.
+        if (trapezoid_adjusted_rate != current_block->nominal_rate) {
+          trapezoid_adjusted_rate = current_block->nominal_rate;
+          set_step_events_per_minute(trapezoid_adjusted_rate);
+        }
+      }
+            
+    } else {   
       // If current block is finished, reset pointer 
-    step_events_completed += 1;
-    if (step_events_completed >= current_block->step_event_count) {
       current_block = NULL;
       plan_discard_current_block();
     }
-  } else {
-    out_bits = 0;
   }
-  out_bits ^= settings.invert_mask;
+  out_bits ^= settings.invert_mask;  // Apply stepper invert mask    
-  
+  busy=false;
-  // In average this generates a trapezoid_generator_tick every CYCLES_PER_ACCELERATION_TICK by keeping track
-  // of the number of elapsed cycles. The code assumes that step_events occur significantly more often than
-  // trapezoid_generator_ticks as they well should. 
-  trapezoid_tick_cycle_counter += cycles_per_step_event;
-  if(trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) {
-    trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK;
-    trapezoid_generator_tick();
 }
 
-  busy=FALSE;
+// This interrupt is set up by ISR_TIMER1_COMPAREA when it sets the motor port bits. It resets
-}
-
-// 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)
+// TODO: It is possible for the serial interrupts to delay this interrupt by a few microseconds, if
+// they execute right before this interrupt. Not a big deal, but could use some TLC at some point.
+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); 
+  TCCR2B = 0; // Disable Timer2 to prevent re-entering this interrupt when it's not needed. 
 }
 
 // Initialize and start the stepper motor subsystem
@@ -217,8 +271,7 @@ void st_init()
   // Configure directions of interface pins
   STEPPING_DDR |= STEPPING_MASK;
   STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
-  LIMIT_DDR &= ~(LIMIT_MASK);
+  STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
-  STEPPERS_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
   
   // waveform generation = 0100 = CTC
   TCCR1B &= ~(1<<WGM13);
@@ -232,17 +285,16 @@ void st_init()
 	
   // Configure Timer 2
   TCCR2A = 0; // Normal operation
-  TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
+  TCCR2B = 0; // Disable timer until needed.
-  TIMSK2 |= (1<<TOIE2);      
+  TIMSK2 |= (1<<TOIE2); // Enable Timer2 interrupt flag
   
-  set_step_events_per_minute(6000);
+  set_step_events_per_minute(MINIMUM_STEPS_PER_MINUTE);
-  DISABLE_STEPPER_DRIVER_INTERRUPT();  
   trapezoid_tick_cycle_counter = 0;
+  current_block = NULL;
+  busy = false;
   
-  // set enable pin     
+  // Start in the idle state
-  STEPPERS_ENABLE_PORT |= 1<<STEPPERS_ENABLE_BIT;
+  st_go_idle();
-     
-  sei();
 }
 
 // Block until all buffered steps are executed
@@ -253,7 +305,7 @@ void st_synchronize()
 
 // 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
-uint32_t config_step_timer(uint32_t cycles)
+static uint32_t config_step_timer(uint32_t cycles)
 {
   uint16_t ceiling;
   uint16_t prescaler;
@@ -291,12 +343,23 @@ uint32_t config_step_timer(uint32_t cycles)
   return(actual_cycles);
 }
 
-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()
 {
-  // Todo: Perform the homing cycle
+  limits_go_home();  
+  plan_set_current_position(0,0,0);
+}
+
+// Planner external interface to start stepper interrupt and execute the blocks in queue.
+void st_cycle_start() 
+{
+  if (!cycle_start) {
+    cycle_start = true;
+    st_wake_up();
+  }
 }

stepper.h

@@ -24,6 +24,11 @@
 #include <avr/io.h>
 #include <avr/sleep.h>
 
+#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
+#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
+#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
+
 // Initialize and start the stepper motor subsystem
 void st_init();
 
@@ -33,8 +38,7 @@ void st_synchronize();
 // Execute the homing cycle
 void st_go_home();
              
-// The stepper subsystem goes to sleep when it runs out of things to execute. Call this
+// Notify the stepper subsystem to start executing the g-code program in buffer.
-// to notify the subsystem that it is time to go to work.
+void st_cycle_start();
-void st_wake_up();
 
 #endif

wiring_serial.c

@@ -1,212 +0,0 @@
-/*
-  wiring_serial.c - serial functions.
-  Part of Arduino - http://www.arduino.cc/
-
-  Copyright (c) 2005-2006 David A. Mellis
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library 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
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General
-  Public License along with this library; if not, write to the
-  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
-  Boston, MA  02111-1307  USA
-
-  $Id: wiring.c 248 2007-02-03 15:36:30Z mellis $
-*/
-
-//#include "wiring_private.h"
-#include <math.h>
-#include <stdlib.h>
-#include <avr/pgmspace.h>
-#include <avr/interrupt.h>
-
-// Define constants and variables for buffering incoming serial data.  We're
-// using a ring buffer (I think), in which rx_buffer_head is the index of the
-// location to which to write the next incoming character and rx_buffer_tail
-// is the index of the location from which to read.
-#ifdef __AVR_ATmega328P__
-#define RX_BUFFER_SIZE 256
-#else
-#define RX_BUFFER_SIZE 64
-#endif
-
-unsigned char rx_buffer[RX_BUFFER_SIZE];
-
-int rx_buffer_head = 0;
-int rx_buffer_tail = 0;
-
-void beginSerial(long baud)
-{
-	UBRR0H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8;
-	UBRR0L = ((F_CPU / 16 + baud / 2) / baud - 1);
-	
-	/* baud doubler off  - Only needed on Uno XXX */
-  UCSR0A &= ~(1 << U2X0);
-          
-	// enable rx and tx
-  UCSR0B |= 1<<RXEN0;
-  UCSR0B |= 1<<TXEN0;
-	
-	// enable interrupt on complete reception of a byte
-  UCSR0B |= 1<<RXCIE0;
-	
-	// defaults to 8-bit, no parity, 1 stop bit
-}
-
-void serialWrite(unsigned char c)
-{
-	while (!(UCSR0A & (1 << UDRE0)))
-		;
-
-	UDR0 = c;
-}
-
-int serialAvailable()
-{
-	return (RX_BUFFER_SIZE + rx_buffer_head - rx_buffer_tail) % RX_BUFFER_SIZE;
-}
-
-int serialRead()
-{
-	// if the head isn't ahead of the tail, we don't have any characters
-	if (rx_buffer_head == rx_buffer_tail) {
-		return -1;
-	} else {
-		unsigned char c = rx_buffer[rx_buffer_tail];
-		rx_buffer_tail = (rx_buffer_tail + 1) % RX_BUFFER_SIZE;
-		return c;
-	}
-}
-
-void serialFlush()
-{
-	// don't reverse this or there may be problems if the RX interrupt
-	// occurs after reading the value of rx_buffer_head but before writing
-	// the value to rx_buffer_tail; the previous value of rx_buffer_head
-	// may be written to rx_buffer_tail, making it appear as if the buffer
-	// were full, not empty.
-	rx_buffer_head = rx_buffer_tail;
-}
-
-SIGNAL(USART_RX_vect)
-{
-	unsigned char c = UDR0;
-	int i = (rx_buffer_head + 1) % RX_BUFFER_SIZE;
-
-	// if we should be storing the received character into the location
-	// just before the tail (meaning that the head would advance to the
-	// current location of the tail), we're about to overflow the buffer
-	// and so we don't write the character or advance the head.
-	if (i != rx_buffer_tail) {
-		rx_buffer[rx_buffer_head] = c;
-		rx_buffer_head = i;
-	}
-}
-
-// void printMode(int mode)
-// {
-//  // do nothing, we only support serial printing, not lcd.
-// }
-
-void printByte(unsigned char c)
-{
-	serialWrite(c);
-}
-
-// void printNewline()
-// {
-//  printByte('\n');
-// }
-// 
-void printString(const char *s)
-{
-	while (*s)
-		printByte(*s++);
-}
-
-// Print a string stored in PGM-memory
-void printPgmString(const char *s)
-{
-  char c;
-	while ((c = pgm_read_byte_near(s++)))
-		printByte(c);
-}
-
-void printIntegerInBase(unsigned long n, unsigned long base)
-{ 
-	unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. 
-	unsigned long i = 0;
-
-	if (n == 0) {
-		printByte('0');
-		return;
-	} 
-
-	while (n > 0) {
-		buf[i++] = n % base;
-		n /= base;
-	}
-
-	for (; i > 0; i--)
-		printByte(buf[i - 1] < 10 ?
-			'0' + buf[i - 1] :
-			'A' + buf[i - 1] - 10);
-}
-
-void printInteger(long n)
-{
-	if (n < 0) {
-		printByte('-');
-		n = -n;
-	}
-
-	printIntegerInBase(n, 10);
-}
-
-void printFloat(double n)
-{
-  double integer_part, fractional_part;
-  fractional_part = modf(n, &integer_part);
-  printInteger(integer_part);
-  printByte('.');
-  printInteger(labs(round(fractional_part*1000)));
-}
-
-// void printHex(unsigned long n)
-// {
-//  printIntegerInBase(n, 16);
-// }
-// 
-// void printOctal(unsigned long n)
-// {
-//  printIntegerInBase(n, 8);
-// }
-// 
-// void printBinary(unsigned long n)
-// {
-//  printIntegerInBase(n, 2);
-// }
-
-/* Including print() adds approximately 1500 bytes to the binary size,
- * so we replace it with the smaller and less-confusing printString(),
- * printInteger(), etc.
-void print(const char *format, ...)
-{
-	char buf[256];
-	va_list ap;
-	
-	va_start(ap, format);
-	vsnprintf(buf, 256, format, ap);
-	va_end(ap);
-	
-	printString(buf);
-}
-*/

wiring_serial.h

@@ -1,45 +0,0 @@
-/*
-  Based on wiring.h - Partial implementation of the Wiring API for the ATmega8.
-  Part of Arduino - http://www.arduino.cc/
-
-  Copyright (c) 2005-2006 David A. Mellis
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library 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
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General
-  Public License along with this library; if not, write to the
-  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
-  Boston, MA  02111-1307  USA
-
-  $Id: wiring.h 387 2008-03-08 21:30:00Z mellis $
-*/
-
-#ifndef wiring_h
-#define wiring_h
-
-void beginSerial(long);
-void serialWrite(unsigned char);
-int serialAvailable(void);
-int serialRead(void);
-void serialFlush(void);
-void printMode(int);
-void printByte(unsigned char c);
-void printNewline(void);
-void printString(const char *s);
-void printPgmString(const char *s);
-void printInteger(long n);
-void printHex(unsigned long n);
-void printOctal(unsigned long n);
-void printBinary(unsigned long n);
-void printIntegerInBase(unsigned long n, unsigned long base);
-void printFloat(double n);
-
-#endif