diff --git a/Makefile b/Makefile
index ccab947..f430a2a 100644
--- a/Makefile
+++ b/Makefile
@@ -28,7 +28,7 @@
 # FUSES ........ Parameters for avrdude to flash the fuses appropriately.
 
 DEVICE     = atmega168
-CLOCK      = 20000000
+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 geometry.o
 FUSES      = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
@@ -77,7 +77,7 @@ main.elf: $(OBJECTS)
 grbl.hex: main.elf
 	rm -f grbl.hex
 	avr-objcopy -j .text -j .data -O ihex main.elf grbl.hex
-	avr-size grbl.hex *.o
+	avr-size *.hex *.elf *.o
 # If you have an EEPROM section, you must also create a hex file for the
 # EEPROM and add it to the "flash" target.
 
diff --git a/arc_algorithm/arc.rb b/arc_algorithm/arc.rb
index 7bb8db4..8501e25 100644
--- a/arc_algorithm/arc.rb
+++ b/arc_algorithm/arc.rb
@@ -238,7 +238,8 @@ class CircleTest
         end
         dx = y.sign*angular_direction unless y == 0
       end
-      break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
+      break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && 
+        y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
     end
     plot_pixel(tx+20, -ty+20, "o")
     return {:tx => tx, :ty => ty, :x => x, :y => y}
diff --git a/arc_algorithm/theta.rb b/arc_algorithm/theta.rb
index 9365ffb..7ee9acd 100644
--- a/arc_algorithm/theta.rb
+++ b/arc_algorithm/theta.rb
@@ -11,6 +11,8 @@ def calc_theta(x,y)
   end
 end
 
-(-180..180).each do |deg|
-  pp [deg, calc_theta(sin(1.0*deg/180*PI), cos(1.0*deg/180*PI))/PI*180]
-end
\ No newline at end of file
+pp calc_theta(5,0)/PI*180;
+
+# (-180..180).each do |deg|
+#   pp [deg, calc_theta(sin(1.0*deg/180*PI), cos(1.0*deg/180*PI))/PI*180]
+# end
\ No newline at end of file
diff --git a/config.h b/config.h
index 8b2594e..f764101 100644
--- a/config.h
+++ b/config.h
@@ -23,40 +23,36 @@
 
 #define VERSION "0.1"
 
-#define X_STEPS_PER_MM 100.0
-#define Y_STEPS_PER_MM 100.0
-#define Z_STEPS_PER_MM 100.0
+#define X_STEPS_PER_MM 5.0
+#define Y_STEPS_PER_MM 5.0
+#define Z_STEPS_PER_MM 5.0
 
 #define INCHES_PER_MM 25.4
 #define X_STEPS_PER_INCH X_STEPS_PER_MM*INCHES_PER_MM
 #define Y_STEPS_PER_INCH Y_STEPS_PER_MM*INCHES_PER_MM
 #define Z_STEPS_PER_INCH Z_STEPS_PER_MM*INCHES_PER_MM
 
-#define RAPID_FEEDRATE 1270.0 // in millimeters per minute
+#define RAPID_FEEDRATE 100.0 // in millimeters per minute
 #define DEFAULT_FEEDRATE 635.0
 
 #define STEPPERS_ENABLE_DDR     DDRB
 #define STEPPERS_ENABLE_PORT    PORTB
 #define STEPPERS_ENABLE_BIT         6
 
-#define MOTORS_DDR       DDRB
-#define MOTORS_PORT      PORTB 
+#define STEPPING_DDR       DDRB
+#define STEPPING_PORT      PORTB 
 #define X_STEP_BIT           0
-#define Y_STEP_BIT           2
-#define Z_STEP_BIT           4
-#define X_DIRECTION_BIT            1
-#define Y_DIRECTION_BIT            3
+#define Y_STEP_BIT           1
+#define Z_STEP_BIT           2
+#define X_DIRECTION_BIT            3
+#define Y_DIRECTION_BIT            4
 #define Z_DIRECTION_BIT            5
-#define STEP_MASK (1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)
-#define DIRECTION_MASK (1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)
-#define MOTORS_MASK STEP_MASK | DIRECTION_MASK
 
 #define LIMIT_DDR      DDRC
 #define LIMIT_PORT     PORTC 
 #define X_LIMIT_BIT          0
 #define Y_LIMIT_BIT          1
 #define Z_LIMIT_BIT          2
-#define LIMIT_MASK (1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)
 
 #define SPINDLE_ENABLE_DDR DDRC
 #define SPINDLE_ENABLE_PORT PORTC
@@ -66,9 +62,14 @@
 #define SPINDLE_DIRECTION_PORT PORTC
 #define SPINDLE_DIRECTION_BIT 4
 
-#define BAUD_RATE 14400
+#define BAUD_RATE 19200
 
-// Unrolling the arc code is faster, but costs about 830 bytes of extra code space.
+// Unrolling the arc code is faster, but consumes about 830 extra bytes of code space.
 // #define UNROLLED_ARC_LOOP
 
+#define STEP_MASK (1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)
+#define DIRECTION_MASK (1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)
+#define STEPPING_MASK STEP_MASK | DIRECTION_MASK
+#define LIMIT_MASK (1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)
+
 #endif
diff --git a/gcode.c b/gcode.c
index 11e7acd..29701d4 100644
--- a/gcode.c
+++ b/gcode.c
@@ -51,6 +51,9 @@
 #include "motion_control.h"
 #include "spindle_control.h"
 #include "geometry.h"
+#include "errno.h"
+
+#include "wiring_serial.h"
 
 #define NEXT_ACTION_DEFAULT 0
 #define NEXT_ACTION_DWELL 1
@@ -82,6 +85,7 @@ struct ParserState {
   uint8_t motion_mode:3;         /* {G0, G1, G2, G3, G38.2, G80, G81, G82, G83, G84, G85, G86, G87, G88, G89} */
   uint8_t inverse_feed_rate_mode:1; /* G93, G94 */
   uint8_t inches_mode:1;         /* 0 = millimeter mode, 1 = inches mode {G20, G21} */
+  uint8_t absolute_mode:1;       /* 0 = relative motion, 1 = absolute motion {G90, G91} */
   uint8_t program_flow:2;
   int spindle_direction:2;
   double feed_rate;              /* Millimeters/second */
@@ -91,9 +95,9 @@ struct ParserState {
   uint8_t plane_axis_0, plane_axis_1; // The axes of the selected plane
   
 };
-struct ParserState state;
+struct ParserState gc;
 
-#define FAIL(status) state.status_code = status;
+#define FAIL(status) gc.status_code = status;
 
 int read_double(char *line, //!< string: line of RS274/NGC code being processed
                      int *counter,       //!< pointer to a counter for position on the line 
@@ -103,44 +107,48 @@ int next_statement(char *letter, double *double_ptr, char *line, int *counter);
 
 
 void gc_init() {
-  memset(&state, 0, sizeof(state));
-  state.feed_rate = DEFAULT_FEEDRATE;
+  memset(&gc, 0, sizeof(gc));
+  gc.feed_rate = DEFAULT_FEEDRATE;
+  gc.plane_axis_0 = X_AXIS; gc.plane_axis_1 = Y_AXIS;
 }
 
 inline float to_millimeters(double value) {
-  return(state.inches_mode ? value * INCHES_PER_MM : value);
+  return(gc.inches_mode ? value * INCHES_PER_MM : value);
 }
 
 void select_plane(uint8_t axis_0, uint8_t axis_1) 
 {
-  state.plane_axis_0 = axis_0;
-  state.plane_axis_1 = axis_1;
+  gc.plane_axis_0 = axis_0;
+  gc.plane_axis_1 = axis_1;
 }
 
 // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
 // characters and signed floats (no whitespace).
 uint8_t gc_execute_line(char *line) {
-  int counter;  
+  int counter = 0;  
   char letter;
   double value;
   double unit_converted_value;
-  double inverse_feed_rate;
-  int radius_mode;
+  double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
+  int radius_mode = false;
   
-  uint8_t absolute_mode;       /* 0 = relative motion, 1 = absolute motion {G90, G91} */
+  uint8_t absolute_override = false;       /* 1 = absolute motion for this block only {G53} */
   uint8_t next_action = NEXT_ACTION_DEFAULT;         /* One of the NEXT_ACTION_-constants */
   
-  double target[3], offset[3];
+  double target[3], offset[3];  
   
-  double p, r;
-  int int_value, axis;
+  double p = 0, r = 0;
+  int int_value;
 
-  state.line_number++;
-  state.status_code = GCSTATUS_OK;
+  clear_vector(target);
+  clear_vector(offset);
+
+  gc.line_number++;
+  gc.status_code = GCSTATUS_OK;
   
   /* First: parse all statements */
   
-  if (line[0] == '(') { return(state.status_code); }
+  if (line[0] == '(') { return(gc.status_code); }
   if (line[0] == '/') { counter++; } // ignore block delete
   
   // Pass 1: Commands
@@ -149,75 +157,81 @@ uint8_t gc_execute_line(char *line) {
     switch(letter) {
       case 'G':
       switch(int_value) {
-        case 0: state.motion_mode = MOTION_MODE_RAPID_LINEAR; break;
-        case 1: state.motion_mode = MOTION_MODE_LINEAR; break;
-        case 2: state.motion_mode = MOTION_MODE_CW_ARC; break;
-        case 3: state.motion_mode = MOTION_MODE_CCW_ARC; break;
+        case 0: gc.motion_mode = MOTION_MODE_RAPID_LINEAR; break;
+        case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
+        case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
+        case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
         case 4: next_action = NEXT_ACTION_DWELL; break;
         case 17: select_plane(X_AXIS, Y_AXIS); break;
         case 18: select_plane(X_AXIS, Z_AXIS); break;
         case 19: select_plane(Y_AXIS, Z_AXIS); break;
-        case 20: state.inches_mode = true; break;
-        case 21: state.inches_mode = false; break;
+        case 20: gc.inches_mode = true; break;
+        case 21: gc.inches_mode = false; break;
         case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
-        case 53: absolute_mode = 1; break;
-        case 80: state.motion_mode = MOTION_MODE_CANCEL; break;
-        case 93: state.inverse_feed_rate_mode = true; break;
-        case 94: state.inverse_feed_rate_mode = false; break;
+        case 53: absolute_override = true; break;
+        case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
+        case 90: gc.absolute_mode = true; break;
+        case 91: gc.absolute_mode = false; break;
+        case 93: gc.inverse_feed_rate_mode = true; break;
+        case 94: gc.inverse_feed_rate_mode = false; break;
         default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
       }
       break;
       
       case 'M':
       switch(int_value) {
-        case 0: case 1: state.program_flow = PROGRAM_FLOW_PAUSED; break;
-        case 2: state.program_flow = PROGRAM_FLOW_COMPLETED; break;
-        case 3: state.spindle_direction = 1; break;
-        case 4: state.spindle_direction = -1; break;
-        case 5: state.spindle_direction = 0; break;
+        case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break;
+        case 2: gc.program_flow = PROGRAM_FLOW_COMPLETED; break;
+        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);
       }            
       break;
-      case 'T': state.tool = trunc(value); break;
+      case 'T': gc.tool = trunc(value); break;
     }
-    if(state.status_code) { break; }
+    if(gc.status_code) { break; }
   }
   
   // If there were any errors parsing this line, we will return right away with the bad news
-  if (state.status_code) { return(state.status_code); }
+  if (gc.status_code) { return(gc.status_code); }
 
   // Pass 2: Parameters
   counter = 0;
   clear_vector(offset);
+  memcpy(target, gc.position, sizeof(target)); // target = gc.position
+
   while(next_statement(&letter, &value, line, &counter)) {
     int_value = trunc(value);
     unit_converted_value = to_millimeters(value);
     switch(letter) {
       case 'F': 
-      if (state.inverse_feed_rate_mode) {
+      if (gc.inverse_feed_rate_mode) {
         inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
       } else {
-        state.feed_rate = unit_converted_value; // millimeters pr second
+        gc.feed_rate = unit_converted_value; // millimeters pr second
       }
       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 'S': state.spindle_speed = value; break;
+      case 'S': gc.spindle_speed = value; break;
       case 'X': case 'Y': case 'Z':
-      axis = letter - 'X';
-      if (absolute_mode) {
-        target[axis] = unit_converted_value;
+      if (gc.absolute_mode || absolute_override) {
+        target[letter - 'X'] = unit_converted_value;
       } else {
-        target[axis] = state.position[axis]+unit_converted_value;
-      };
+        target[letter - 'X'] += unit_converted_value;
+      }
       break;
     }
   }
   
+  // If there were any errors parsing this line, we will return right away with the bad news
+  if (gc.status_code) { return(gc.status_code); }
+    
   // Update spindle state
-  if (state.spindle_direction) {
-    spindle_run(state.spindle_direction, state.spindle_speed);
+  if (gc.spindle_direction) {
+    spindle_run(gc.spindle_direction, gc.spindle_speed);
   } else {
     spindle_stop();
   }
@@ -227,7 +241,7 @@ uint8_t gc_execute_line(char *line) {
     case NEXT_ACTION_GO_HOME: mc_go_home(); break;
     case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break;
     case NEXT_ACTION_DEFAULT: 
-    switch (state.motion_mode) {
+    switch (gc.motion_mode) {
       case MOTION_MODE_CANCEL: break;
       case MOTION_MODE_RAPID_LINEAR: case MOTION_MODE_LINEAR:
       if (inverse_feed_rate) {
@@ -235,7 +249,7 @@ uint8_t gc_execute_line(char *line) {
           inverse_feed_rate, true);
       } else {
         mc_linear_motion(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], 
-          (state.motion_mode == MOTION_MODE_LINEAR) ? state.feed_rate : RAPID_FEEDRATE,
+          (gc.motion_mode == MOTION_MODE_LINEAR) ? gc.feed_rate : RAPID_FEEDRATE,
           false);
       }
       break;
@@ -244,15 +258,14 @@ uint8_t gc_execute_line(char *line) {
         /* 
           We need to calculate the center of the circle that has the designated radius and passes
           through both the current position and the target position. This method calculates the following
-          set of equations where [x,y] is the vector from current to target position, d == distance of 
+          set of equations where [x,y] is the vector from current to target position, d == magnitude of 
           that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
-          the center of the travel vector. This is the distance from the center of the travel vector
-          to the center of our circle. A perpendicular to the travel vector is scaled to the length of
-          h and added to the center of the travel vector to form the new point [i,j] which will be 
-          the center of our circle.
+          the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the 
+          length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point 
+          [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
           
           d^2 == x^2 + y^2
-          h^2 == r^2 + (d/2)^2
+          h^2 == r^2 - (d/2)^2
           i == x/2 - y/d*h
           j == y/2 + x/d*h
           
@@ -273,35 +286,60 @@ uint8_t gc_execute_line(char *line) {
           h - distance from center of CT to O
           
           Expanding the equations:
-          
-          h = sqrt(4 r^2 + x^2 + y^2)/2
-          d = sqrt(x^2 + y^2)
-          i = x/2 - (h * y)/d
-          j = y/2 + (h * x)/d
-          
+
+          d -> sqrt(x^2 + y^2)
+          h -> sqrt(4 * r^2 - x^2 - y^2)/2
+          i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 
+          j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
+         
           Which can be written:
           
-          i = (x - (y * sqrt(4 * r^2 + x^2 + y^2))/sqrt(x^2 + y^2))/2
-          j = (y + (x * sqrt(4 * r^2 + x^2 + y^2))/sqrt(x^2 + y^2))/2
+          i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
+          j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
           
-          Which can be optimized to:
+          Which we for size and speed reasons optimize to:
 
-          h_x2_div_d = sqrt(4 * r^2 + x^2 + y^2)/sqrt(x^2 + y^2)
+          h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
           i = (x - (y * h_x2_div_d))/2
           j = (y + (x * h_x2_div_d))/2
           
         */
         
         // Calculate the change in position along each selected axis
-        double x = target[state.plane_axis_0]-state.position[state.plane_axis_0];
-        double y = target[state.plane_axis_1]-state.position[state.plane_axis_1];
+        double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
+        double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
         clear_vector(&offset);
-        double h_x2_div_d = sqrt(4*r*r + x*x + y*y)/hypot(x,y); // == h * 2 / d
-        // The anti-clockwise circle lies to the right of the target direction. When offset is positive,
-        // the left hand circle will be generated - when it is negative the right hand circle is generated.
-        if (state.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
-        offset[state.plane_axis_0] = (x-(y*h_x2_div_d))/2;
-        offset[state.plane_axis_1] = (y+(x*h_x2_div_d))/2;
+        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
+        // earthly 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); }
+
+        /* The anti-clockwise circle lies to the right of the target direction. When offset is positive,
+           the left hand circle will be generated - when it is negative the right hand circle is generated.
+           
+           
+                                                         T
+                                                         
+                                                         ^
+                                                         |
+                                                         |
+  center of arc when h_x2_div_d is positive ->  x <----- | -----> x <- center of arc when h_x2_div_d is negative
+                                                         |
+                                                         |
+                                                         
+                                                         C                                                     */
+        
+        if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
+        
+        // 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_1] = (y+(x*h_x2_div_d))/2;
+        
+        // printByte('(');
+        // printInteger(trunc(offset[gc.plane_axis_0])); printByte(',');
+        // printInteger(trunc(offset[gc.plane_axis_1]));
+        // printByte(')');
+        
       } 
       
       /*
@@ -313,28 +351,34 @@ uint8_t gc_execute_line(char *line) {
                           * * *                
                         *       *                                               
                       *           *                                             
-                      *     O ----T   <- theta == theta_end (e.g. 90 degrees: theta == PI/2)                                          
+                      *     O ----T   <- theta_end (e.g. 90 degrees: theta_end == PI/2)                                          
                       *   /                                                     
-                        C   <- theta == theta_start (e.g. -145 degrees: theta == -PI*(3/4))
+                        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[state.plane_axis_0], -offset[state.plane_axis_1]);
+      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[state.plane_axis_0] - offset[state.plane_axis_0] - state.position[state.plane_axis_0], 
-        target[state.plane_axis_1] - offset[state.plane_axis_1] - state.position[state.plane_axis_1]);
+      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]);
+      // double theta_end = theta(5,0);
       // 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 (state.motion_mode == MOTION_MODE_CCW_ARC) {
+      if (gc.motion_mode == MOTION_MODE_CCW_ARC) {
         angular_travel = angular_travel-2*M_PI;
       }
       // Find the radius
-      double radius = hypot(offset[state.plane_axis_0], offset[state.plane_axis_1]);
+      double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
       // Prepare the arc
-      mc_arc(theta_start, angular_travel, radius, state.plane_axis_0, state.plane_axis_1, state.feed_rate);        
+      printString("mc_arc(");
+      printInteger(trunc(theta_start/M_PI*180)); printByte(',');
+      printInteger(trunc(angular_travel/M_PI*180)); printByte(',');
+      printInteger(trunc(radius)); printByte('…');
+      printByte(')');
+      mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate);        
       break;      
     }    
   }
@@ -344,40 +388,39 @@ uint8_t gc_execute_line(char *line) {
   // As far as the parser is concerned, the position is now == target. In reality the
   // motion control system might still be processing the action and the real tool position
   // in any intermediate location.
-  memcpy(state.position, target, sizeof(state.position));
-  
-  return(state.status_code);
+  memcpy(gc.position, target, sizeof(double)*3);
+  return(gc.status_code);
 }
 
 void gc_get_status(double *position, uint8_t *status_code, int *inches_mode, uint32_t *line_number) 
 {
   int axis;
-  if (state.inches_mode) {
+  if (gc.inches_mode) {
     for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
-      position[axis] = state.position[axis]*INCHES_PER_MM;
+      position[axis] = gc.position[axis]*INCHES_PER_MM;
     }
   } else {
-    memcpy(position, state.position, sizeof(position));    
+    memcpy(position, gc.position, sizeof(gc.position));    
   }
-  *status_code = state.status_code;
-  *inches_mode = state.inches_mode;
-  *line_number = state.line_number;
+  *status_code = gc.status_code;
+  *inches_mode = gc.inches_mode;
+  *line_number = gc.line_number;
 }
 
 // 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 *counter) {
-  if (*line == 0) {
+  if (line[*counter] == 0) {
     return(0); // No more statements
   }
   
-  *letter = *line;
+  *letter = line[*counter];
   if((*letter < 'A') || (*letter > 'Z')) {
     FAIL(GCSTATUS_EXPECTED_COMMAND_LETTER);
     return(0);
   }
-  *counter++;
+  (*counter)++;
   if (!read_double(line, counter, double_ptr)) {
     return(0);
   };
@@ -400,4 +443,3 @@ int read_double(char *line, //!< string: line of RS274/NGC code being processed
   *counter = end - line;
   return(1);
 }
-
diff --git a/gcode.h b/gcode.h
index f22735a..52f2763 100644
--- a/gcode.h
+++ b/gcode.h
@@ -28,6 +28,7 @@
 #define GCSTATUS_EXPECTED_COMMAND_LETTER 2
 #define GCSTATUS_UNSUPPORTED_STATEMENT 3
 #define GCSTATUS_MOTION_CONTROL_ERROR 4
+#define GCSTATUS_FLOATING_POINT_ERROR 5
 
 // Initialize the parser
 void gc_init();
diff --git a/geometry.c b/geometry.c
index fa387de..75e3151 100644
--- a/geometry.c
+++ b/geometry.c
@@ -20,7 +20,8 @@
 
 #include <math.h>
 
-// Find the angle from the positive y axis to the given point with respect to origo.
+// 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));
@@ -29,10 +30,9 @@ double theta(double x, double y)
   } else {
     if (theta>0) 
     {
-      return(theta-M_PI);
+      return(M_PI-theta);
     } else {
       return(-M_PI-theta);
     }
   }
 }
-
diff --git a/geometry.h b/geometry.h
index ba06dc6..0a604f6 100644
--- a/geometry.h
+++ b/geometry.h
@@ -24,4 +24,4 @@
 // Find the angle from the positive y axis to the given point with respect to origo.
 double theta(double x, double y);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/legacy/unrolled_arc.c b/legacy/unrolled_arc.c
new file mode 100644
index 0000000..807dbe8
--- /dev/null
+++ b/legacy/unrolled_arc.c
@@ -0,0 +1,220 @@
+// Prepare 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 plane in tool space. 
+// ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis.
+void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate)
+{  
+  uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
+  mc.mode = MC_MODE_ARC;
+  // Determine angular direction (+1 = clockwise, -1 = counterclockwise)
+  mc.arc.angular_direction = signof(angular_travel);
+  // Calculate the initial position and target position in the local coordinate system of the arc
+  mc.arc.x = round(sin(theta)*radius_steps); 
+  mc.arc.y = round(cos(theta)*radius_steps);
+  mc.arc.target_x = trunc(sin(theta+angular_travel)*radius_steps);
+  mc.arc.target_y = trunc(cos(theta+angular_travel)*radius_steps);
+  // Precalculate these values to optimize target detection
+  mc.arc.target_direction_x = signof(mc.arc.target_x)*mc.arc.angular_direction;
+  mc.arc.target_direction_y = signof(mc.arc.target_y)*mc.arc.angular_direction;
+  // The "error" factor is kept up to date so that it is always == (x**2+y**2-radius**2). When error 
+  // <0 we are inside the arc, when it is >0 we are outside of the arc, and when it is 0 we 
+  // are exactly on top of the arc.
+  mc.arc.error = mc.arc.x*mc.arc.x + mc.arc.y*mc.arc.y - radius_steps*radius_steps;
+  // Because the error-value moves in steps of (+/-)2x+1 and (+/-)2y+1 we save a couple of multiplications
+  // by keeping track of the doubles of the arc coordinates at all times.
+  mc.arc.x2 = 2*mc.arc.x;
+  mc.arc.y2 = 2*mc.arc.y; 
+  
+  // Set up a vector with the steppers we are going to use tracing the plane of this arc
+  clear_vector(mc.arc.plane_steppers);
+  mc.arc.plane_steppers[axis_1] = 1;
+  mc.arc.plane_steppers[axis_2] = 1;
+  // And map the local coordinate system of the arc onto the tool axes of the selected plane
+  mc.arc.axis_x = axis_1;
+  mc.arc.axis_y = axis_2;
+  // mm/second -> microseconds/step. Assumes all axes have the same steps/mm as the x axis
+  mc.pace = 
+    ONE_MINUTE_OF_MICROSECONDS / (feed_rate * X_STEPS_PER_MM);
+  mc.arc.incomplete = true;
+}
+
+#define check_arc_target \
+  if ((mc.arc.x * mc.arc.target_direction_y >= \
+          mc.arc.target_x * mc.arc.target_direction_y) && \
+         (mc.arc.y * mc.arc.target_direction_x <= \
+          mc.arc.target_y * mc.arc.target_direction_x)) \
+  { mc.arc.incomplete = false; }
+
+// Internal method used by execute_arc to trace horizontally in the general direction provided by dx and dy
+void step_arc_along_x(int8_t dx, int8_t dy) 
+{
+  uint32_t diagonal_error;
+  mc.arc.x+=dx;
+  mc.arc.error += 1+mc.arc.x2*dx;
+  mc.arc.x2 += 2*dx;
+  diagonal_error = mc.arc.error + 1 + mc.arc.y2*dy;
+  if(abs(mc.arc.error) >= abs(diagonal_error)) {
+    mc.arc.y += dy;
+    mc.arc.y2 += 2*dy;
+    mc.arc.error = diagonal_error;
+    step_steppers(mc.arc.plane_steppers); // step diagonal
+  } else {
+    step_axis(mc.arc.axis_x); // step straight
+  }
+  check_arc_target;
+}
+
+// Internal method used by execute_arc to trace vertically in the general direction provided by dx and dy
+void step_arc_along_y(int8_t dx, int8_t dy) 
+{  
+  uint32_t diagonal_error;
+  mc.arc.y+=dy; 
+  mc.arc.error += 1+mc.arc.y2*dy; 
+  mc.arc.y2 += 2*dy; 
+  diagonal_error = mc.arc.error + 1 + mc.arc.x2*dx; 
+  if(abs(mc.arc.error) >= abs(diagonal_error)) { 
+    mc.arc.x += dx; 
+    mc.arc.x2 += 2*dx; 
+    mc.arc.error = diagonal_error; 
+    step_steppers(mc.arc.plane_steppers); // step diagonal
+  } else {
+    step_axis(mc.arc.axis_y); // step straight
+  }
+  check_arc_target;
+}
+
+// Take dx and dy which are local to the arc being generated and map them on to the 
+// selected tool-space-axes for the current arc.
+void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
+{
+  int8_t direction[3];
+  direction[mc.arc.axis_x] = dx;
+  direction[mc.arc.axis_y] = dy;
+  set_stepper_directions(direction);
+}
+
+
+
+/*
+ Quandrants of the arc
+       \ 7|0 /
+        \ | / 
+      6  \|/  1    y+
+ ---------|-----------
+      5  /|\  2    y-
+        / | \  
+   x-  / 4|3 \ x+           */
+
+#ifdef UNROLLED_ARC_LOOP // This function only used by the unrolled arc loop
+// Determine within which quadrant of the circle the provided coordinate falls
+int quadrant(uint32_t x,uint32_t y)
+{
+  // determine if the coordinate is in the quadrants 0,3,4 or 7
+  register int quad0347 = abs(x)<abs(y);
+  
+  if (x<0) { // quad 4567
+    if (y<0) { // quad 45
+      return(quad0347 ? 4 : 5);
+    } else { // quad 67
+      return(quad0347 ? 7 : 6);
+    }
+  } else {
+    if (y<0) { // quad 23
+      return(quad0347 ? 3 : 2);
+    } else { // quad 01
+      return(quad0347 ? 0 : 1);
+    }
+  }  
+}
+#endif
+
+// Will trace the configured arc until the target is reached. 
+void execute_arc()
+{
+  uint32_t start_x = mc.arc.x;
+  uint32_t start_y = mc.arc.y;
+  int dx, dy; // Trace directions
+  int steps = 0;
+
+  // mc.mode is set to 0 (MC_MODE_AT_REST) when target is reached
+  while(mc.arc.incomplete && (steps<400))
+  {
+    steps++;
+#ifdef UNROLLED_ARC_LOOP
+    // Unrolling the arc code is fast, but costs about 830 bytes of extra code space.
+    int q = quadrant(mc.arc.x, mc.arc.y);
+    if (mc.arc.angular_direction) {
+      switch (q) {
+        case 0: 
+        map_local_arc_directions_to_stepper_directions(1,-1);
+        while(mc.arc.incomplete && (mc.arc.x>mc.arc.y)) { step_arc_along_x(1,-1); }
+        case 1: 
+        map_local_arc_directions_to_stepper_directions(1,-1);
+        while(mc.arc.incomplete && (mc.arc.y>0)) { step_arc_along_y(1,-1); }
+        case 2: 
+        map_local_arc_directions_to_stepper_directions(-1,-1);
+        while(mc.arc.incomplete && (mc.arc.y>-mc.arc.x)) { step_arc_along_y(-1,-1); }
+        case 3: 
+        map_local_arc_directions_to_stepper_directions(-1,-1);
+        while(mc.arc.incomplete && (mc.arc.x>0)) { step_arc_along_x(-1,-1); }
+        case 4: 
+        map_local_arc_directions_to_stepper_directions(-1,1);
+        while(mc.arc.incomplete && (mc.arc.y<mc.arc.x)) { step_arc_along_x(-1,1); }
+        case 5: 
+        map_local_arc_directions_to_stepper_directions(-1,1);
+        while(mc.arc.incomplete && (mc.arc.y<0)) { step_arc_along_y(-1,1); }
+        case 6: 
+        map_local_arc_directions_to_stepper_directions(1,-1);
+        while(mc.arc.incomplete && (mc.arc.y<-mc.arc.x)) { step_arc_along_y(1,1); }
+        case 7: 
+        map_local_arc_directions_to_stepper_directions(1,1);
+        while(mc.arc.incomplete && (mc.arc.x<0)) { step_arc_along_x(1,1); }
+      }
+    } else {
+      switch (q) {
+        case 7:
+        map_local_arc_directions_to_stepper_directions(-1,-1);
+        while(mc.arc.incomplete && (mc.arc.y>-mc.arc.x)) { step_arc_along_x(-1,-1); }
+        case 6:
+        map_local_arc_directions_to_stepper_directions(-1,-1);
+        while(mc.arc.incomplete && (mc.arc.y>0))  { step_arc_along_y(-1,-1); }
+        case 5:
+        map_local_arc_directions_to_stepper_directions(1,-1);
+        while(mc.arc.incomplete && (mc.arc.y>mc.arc.x))  { step_arc_along_y(1,-1); }
+        case 4:
+        map_local_arc_directions_to_stepper_directions(1,-1);
+        while(mc.arc.incomplete && (mc.arc.x<0))  { step_arc_along_x(1,-1); }
+        case 3:
+        map_local_arc_directions_to_stepper_directions(1,1);
+        while(mc.arc.incomplete && (mc.arc.y<-mc.arc.x)) { step_arc_along_x(1,1); }      
+        case 2:
+        map_local_arc_directions_to_stepper_directions(1,1);
+        while(mc.arc.incomplete && (mc.arc.y<0))  { step_arc_along_y(1,1); }      
+        case 1:
+        map_local_arc_directions_to_stepper_directions(-1,1);
+        while(mc.arc.incomplete && (mc.arc.y<mc.arc.x))  { step_arc_along_y(-1,1); }
+        case 0:
+        map_local_arc_directions_to_stepper_directions(-1,1);
+        while(mc.arc.incomplete && (mc.arc.x>0))  { step_arc_along_x(-1,1); }
+      }    
+    }
+#else 
+    dx = (mc.arc.y!=0) ?  signof(mc.arc.y) * mc.arc.angular_direction : -signof(mc.arc.x);
+    dy = (mc.arc.x!=0) ? -signof(mc.arc.x) * mc.arc.angular_direction : -signof(mc.arc.y);
+    map_local_arc_directions_to_stepper_directions(dx,dy);
+    if (abs(mc.arc.x)<abs(mc.arc.y)) {
+      step_arc_along_x(dx,dy);    
+    } else {
+      
+      step_arc_along_y(dx,dy);    
+    }
+#endif    
+  }
+  // Update the tool position to the new actual position
+  mc.position[mc.arc.axis_x] += mc.arc.x-start_x;
+  mc.position[mc.arc.axis_y] += mc.arc.y-start_y;
+  // Because of rounding errors we might be off by a step or two. Adjust for this
+    // To be implemented
+  //void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
+  mc.mode = MC_MODE_AT_REST;  
+}
diff --git a/main.c b/main.c
index 55cbcc4..3392d9f 100644
--- a/main.c
+++ b/main.c
@@ -20,14 +20,19 @@
 
 #include <avr/io.h>
 #include <avr/sleep.h>
+#include <util/delay.h>
 #include "stepper.h"
 #include "spindle_control.h"
 #include "motion_control.h"
 #include "gcode.h"
 #include "serial_protocol.h"
 
+#include "config.h"
+#include "wiring_serial.h"
+
 int main(void)
 {
+  beginSerial(BAUD_RATE);
   st_init();
   mc_init(); // initialize motion control subsystem
   spindle_init(); // initialize spindle controller
@@ -35,7 +40,7 @@ int main(void)
   sp_init(); // initialize the serial protocol
   
   for(;;){
-    sleep_mode();
+//    sleep_mode();
     sp_process(); // process the serial protocol
   }
   return 0;   /* never reached */
diff --git a/motion_control.c b/motion_control.c
index 50cf1af..8a5f8b1 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -35,6 +35,8 @@
 #include "nuts_bolts.h"
 #include "stepper.h"
 
+#include "wiring_serial.h"
+
 #define ONE_MINUTE_OF_MICROSECONDS 60000000
 
 // Parameters when mode is MC_MODE_ARC
@@ -49,10 +51,10 @@ struct LinearMotionParameters {
 
 struct ArcMotionParameters {
   int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise
-  uint32_t x, y, target_x, target_y;               // current position and target position in the 
-                                                   // local coordinate system of the arc where [0,0] is the 
+  int32_t x, y, target_x, target_y;                // current position and target position in the 
+                                                   // local coordinate system of the arc-generator where [0,0] is the 
                                                    // center of the arc.
-  int target_direction_x, target_direction_y;      // sign(target_x)*angular_direction precalculated for speed                                                 
+  int target_direction_x, target_direction_y;      // signof(target_x)*angular_direction precalculated for speed                                                 
   int32_t error, x2, y2;                           // error is always == (x**2 + y**2 - radius**2), 
                                                    // x2 is always 2*x, y2 is always 2*y
   uint8_t axis_x, axis_y;                          // maps the arc axes to stepper axes
@@ -69,31 +71,41 @@ struct MotionControlState {
   int8_t mode;            // The current operation mode
   int32_t position[3];    // The current position of the tool in absolute steps
   int32_t pace;  // Microseconds between each update in the current mode
+  uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
   union {
     struct LinearMotionParameters linear; // variables used in MC_MODE_LINEAR
     struct ArcMotionParameters arc;       // variables used in MC_MODE_ARC
     uint32_t dwell_milliseconds;          // variable used in MC_MODE_DWELL
   };
 };
-struct MotionControlState state;
+struct MotionControlState mc;
 
-uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
 
 void set_stepper_directions(int8_t *direction);
 inline void step_steppers(uint8_t *enabled);
 inline void step_axis(uint8_t axis);
 void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate);
 
+// void printCurrentPosition() {
+//   int axis;
+//   printString("[ ");  
+//   for(axis=X_AXIS; axis<=Z_AXIS; axis++) {
+//     printInteger(trunc(mc.position[axis]*100));
+//     printByte(' ');
+//   }
+//   printString("]\n\r");  
+// }
+// 
 void mc_init()
 {
 	// Initialize state variables
-  memset(&state, 0, sizeof(state));
+  memset(&mc, 0, sizeof(mc));
 }
 
 void mc_dwell(uint32_t milliseconds) 
 {
-  state.mode = MC_MODE_DWELL;
-  state.dwell_milliseconds = milliseconds;
+  mc.mode = MC_MODE_DWELL;
+  mc.dwell_milliseconds = milliseconds;
 }
 
 // Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
@@ -105,39 +117,45 @@ void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_
 
 // Same as mc_linear_motion but accepts target in absolute step coordinates
 void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
-{  
-  state.mode = MC_MODE_LINEAR;
+{
+  mc.linear.target[X_AXIS] = x;
+  mc.linear.target[Y_AXIS] = y;
+  mc.linear.target[Z_AXIS] = z;
+  
+  mc.mode = MC_MODE_LINEAR;
   uint8_t axis; // loop variable
-
   // Determine direction and travel magnitude for each axis
   for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
-  	state.linear.step_count[axis] = abs(state.linear.target[axis] - state.position[axis]);
-    state.linear.direction[axis] = sign(state.linear.step_count[axis]);
+  	mc.linear.step_count[axis] = abs(mc.linear.target[axis] - mc.position[axis]);
+    mc.linear.direction[axis] = signof(mc.linear.target[axis] - mc.position[axis]);
   }
   // Find the magnitude of the axis with the longest travel
-	state.linear.maximum_steps = max(state.linear.step_count[Z_AXIS], 
-	  max(state.linear.step_count[X_AXIS], state.linear.step_count[Y_AXIS]));
-
+	mc.linear.maximum_steps = max(mc.linear.step_count[Z_AXIS], 
+	  max(mc.linear.step_count[X_AXIS], mc.linear.step_count[Y_AXIS]));
+  // Nothing to do?
+	if ((mc.linear.maximum_steps) == 0) 
+	{ 
+    mc.mode = MC_MODE_AT_REST;
+    return; 
+	}
 	// Set up a neat counter for each axis
   for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
-    state.linear.counter[axis] = -state.linear.maximum_steps/2;
+    mc.linear.counter[axis] = -mc.linear.maximum_steps/2;
   }
-
 	// Set our direction pins 
-  set_stepper_directions(state.linear.direction);
-
+  set_stepper_directions(mc.linear.direction);
   // Calculate the microseconds we need to wait between each step to achieve the desired feed rate
   if (invert_feed_rate) {
-  	state.pace = 
-  	  (feed_rate*1000000)/state.linear.maximum_steps;	  
+  	mc.pace = 
+  	  (feed_rate*1000000)/mc.linear.maximum_steps;	  
   } else {
   	// Ask old Phytagoras to estimate how many steps our next move is going to take us:
   	uint32_t steps_to_travel = 
-      ceil(sqrt(pow((X_STEPS_PER_MM*state.linear.step_count[X_AXIS]),2) + 
-                pow((Y_STEPS_PER_MM*state.linear.step_count[Y_AXIS]),2) + 
-                pow((Z_STEPS_PER_MM*state.linear.step_count[Z_AXIS]),2)));
-  	state.pace = 
-  	  ((steps_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / state.linear.maximum_steps;	  
+      ceil(sqrt(pow((X_STEPS_PER_MM*mc.linear.step_count[X_AXIS]),2) + 
+                pow((Y_STEPS_PER_MM*mc.linear.step_count[Y_AXIS]),2) + 
+                pow((Z_STEPS_PER_MM*mc.linear.step_count[Z_AXIS]),2)));
+  	mc.pace = 
+  	  ((steps_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / mc.linear.maximum_steps;	  
   }
 }
 
@@ -147,27 +165,27 @@ void execute_linear_motion()
   uint8_t step[3];
   uint8_t axis; // loop variable
   
-	// Trace the line
-  clear_vector(step);
-  for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
-		if (state.linear.target[axis] != state.position[axis])
-		{
-			state.linear.counter[axis] += state.linear.step_count[axis];
-			if (state.linear.counter[axis] > 0)
-			{
-        step[axis] = true;
-				state.linear.counter[axis] -= state.linear.maximum_steps;
-        state.position[axis] += state.linear.direction[axis];
-			}
-		}
-	}
-
-	if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) {
-    step_steppers(step);
-
-	} else {
-    state.mode = MC_MODE_AT_REST;
-	}
+  while(mc.mode) {
+  	// Trace the line
+    clear_vector(step);
+    for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
+  		if (mc.linear.target[axis] != mc.position[axis])
+  		{
+  			mc.linear.counter[axis] += mc.linear.step_count[axis];
+  			if (mc.linear.counter[axis] > 0)
+  			{
+          step[axis] = true;
+  				mc.linear.counter[axis] -= mc.linear.maximum_steps;
+          mc.position[axis] += mc.linear.direction[axis];
+  			}
+  		}
+  	}
+  	if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) {
+      step_steppers(step);
+  	} else {
+      mc.mode = MC_MODE_AT_REST;
+  	}
+  }
 }
 
 // Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc,
@@ -176,61 +194,62 @@ void execute_linear_motion()
 // ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis.
 void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate)
 {  
-  state.mode = MC_MODE_ARC;
+  uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
+  mc.mode = MC_MODE_ARC;
   // Determine angular direction (+1 = clockwise, -1 = counterclockwise)
-  state.arc.angular_direction = sign(angular_travel);
+  mc.arc.angular_direction = signof(angular_travel);
   // Calculate the initial position and target position in the local coordinate system of the arc
-  state.arc.x = round(sin(theta)*radius*X_STEPS_PER_MM); 
-  state.arc.y = round(cos(theta)*radius*X_STEPS_PER_MM);
-  state.arc.target_x = trunc(sin(theta+angular_travel)*(radius*X_STEPS_PER_MM-0.5));
-  state.arc.target_y = trunc(cos(theta+angular_travel)*(radius*X_STEPS_PER_MM-0.5));
+  mc.arc.x = round(sin(theta)*radius_steps); 
+  mc.arc.y = round(cos(theta)*radius_steps);
+  mc.arc.target_x = trunc(sin(theta+angular_travel)*radius_steps);
+  mc.arc.target_y = trunc(cos(theta+angular_travel)*radius_steps);
   // Precalculate these values to optimize target detection
-  state.arc.target_direction_x = sign(state.arc.target_x)*state.arc.angular_direction;
-  state.arc.target_direction_y = sign(state.arc.target_y)*state.arc.angular_direction;
+  mc.arc.target_direction_x = signof(mc.arc.target_x)*mc.arc.angular_direction;
+  mc.arc.target_direction_y = signof(mc.arc.target_y)*mc.arc.angular_direction;
   // The "error" factor is kept up to date so that it is always == (x**2+y**2-radius**2). When error 
   // <0 we are inside the arc, when it is >0 we are outside of the arc, and when it is 0 we 
   // are exactly on top of the arc.
-  state.arc.error = round(pow(state.arc.x,2) + pow(state.arc.y,2) - pow(radius,2));
+  mc.arc.error = mc.arc.x*mc.arc.x + mc.arc.y*mc.arc.y - radius_steps*radius_steps;
   // Because the error-value moves in steps of (+/-)2x+1 and (+/-)2y+1 we save a couple of multiplications
   // by keeping track of the doubles of the arc coordinates at all times.
-  state.arc.x2 = 2*state.arc.x;
-  state.arc.y2 = 2*state.arc.y; 
+  mc.arc.x2 = 2*mc.arc.x;
+  mc.arc.y2 = 2*mc.arc.y; 
   
   // Set up a vector with the steppers we are going to use tracing the plane of this arc
-  clear_vector(state.arc.plane_steppers);
-  state.arc.plane_steppers[axis_1] = 1;
-  state.arc.plane_steppers[axis_2] = 1;
+  clear_vector(mc.arc.plane_steppers);
+  mc.arc.plane_steppers[axis_1] = 1;
+  mc.arc.plane_steppers[axis_2] = 1;
   // And map the local coordinate system of the arc onto the tool axes of the selected plane
-  state.arc.axis_x = axis_1;
-  state.arc.axis_y = axis_2;
+  mc.arc.axis_x = axis_1;
+  mc.arc.axis_y = axis_2;
   // mm/second -> microseconds/step. Assumes all axes have the same steps/mm as the x axis
-  state.pace = 
+  mc.pace = 
     ONE_MINUTE_OF_MICROSECONDS / (feed_rate * X_STEPS_PER_MM);
-  state.arc.incomplete = true;
+  mc.arc.incomplete = true;
 }
 
 #define check_arc_target \
-  if ((state.arc.x * state.arc.target_direction_y >= \
-          state.arc.target_x * state.arc.target_direction_y) && \
-         (state.arc.y * state.arc.target_direction_x <= \
-          state.arc.target_y * state.arc.target_direction_x)) \
-  { state.arc.incomplete = false; }
+  if ((mc.arc.x * mc.arc.target_direction_y >= \
+          mc.arc.target_x * mc.arc.target_direction_y) && \
+         (mc.arc.y * mc.arc.target_direction_x <= \
+          mc.arc.target_y * mc.arc.target_direction_x)) \
+  { mc.arc.incomplete = false; }
 
 // Internal method used by execute_arc to trace horizontally in the general direction provided by dx and dy
 void step_arc_along_x(int8_t dx, int8_t dy) 
 {
   uint32_t diagonal_error;
-  state.arc.x+=dx;
-  state.arc.error += 1+state.arc.x2*dx;
-  state.arc.x2 += 2*dx;
-  diagonal_error = state.arc.error + 1 + state.arc.y2*dy;
-  if(abs(state.arc.error) < abs(diagonal_error)) {
-    state.arc.y += dy;
-    state.arc.y2 += 2*dy;
-    state.arc.error = diagonal_error;
-    step_steppers(state.arc.plane_steppers); // step diagonal
+  mc.arc.x+=dx;
+  mc.arc.error += 1+mc.arc.x2*dx;
+  mc.arc.x2 += 2*dx;
+  diagonal_error = mc.arc.error + 1 + mc.arc.y2*dy;
+  if(abs(mc.arc.error) >= abs(diagonal_error)) {
+    mc.arc.y += dy;
+    mc.arc.y2 += 2*dy;
+    mc.arc.error = diagonal_error;
+    step_steppers(mc.arc.plane_steppers); // step diagonal
   } else {
-    step_axis(state.arc.axis_x); // step straight
+    step_axis(mc.arc.axis_x); // step straight
   }
   check_arc_target;
 }
@@ -239,172 +258,77 @@ void step_arc_along_x(int8_t dx, int8_t dy)
 void step_arc_along_y(int8_t dx, int8_t dy) 
 {  
   uint32_t diagonal_error;
-  state.arc.y+=dy; 
-  state.arc.error += 1+state.arc.y2*dy; 
-  state.arc.y2 += 2*dy; 
-  diagonal_error = state.arc.error + 1 + state.arc.x2*dx; 
-  if(abs(state.arc.error) < abs(diagonal_error)) { 
-    state.arc.x += dx; 
-    state.arc.x2 += 2*dx; 
-    state.arc.error = diagonal_error; 
-    step_steppers(state.arc.plane_steppers); // step diagonal
+  mc.arc.y+=dy; 
+  mc.arc.error += 1+mc.arc.y2*dy; 
+  mc.arc.y2 += 2*dy; 
+  diagonal_error = mc.arc.error + 1 + mc.arc.x2*dx; 
+  if(abs(mc.arc.error) >= abs(diagonal_error)) { 
+    mc.arc.x += dx; 
+    mc.arc.x2 += 2*dx; 
+    mc.arc.error = diagonal_error; 
+    step_steppers(mc.arc.plane_steppers); // step diagonal
   } else {
-    step_axis(state.arc.axis_y); // step straight
+    step_axis(mc.arc.axis_y); // step straight
   }
   check_arc_target;
 }
 
-// Take dx and dy which are local to the arc being generated and map them on to the 
-// selected tool-space-axes for the current arc.
-void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
-{
-  int8_t direction[3];
-  direction[state.arc.axis_x] = dx;
-  direction[state.arc.axis_y] = dy;
-  set_stepper_directions(direction);
-}
-
-
-
-/*
- Quandrants of the arc
-       \ 7|0 /
-        \ | / 
-      6  \|/  1    y+
- ---------|-----------
-      5  /|\  2    y-
-        / | \  
-   x-  / 4|3 \ x+           */
-
-#ifdef UNROLLED_ARC_LOOP // This function only used by the unrolled arc loop
-// Determine within which quadrant of the circle the provided coordinate falls
-int quadrant(uint32_t x,uint32_t y)
-{
-  // determine if the coordinate is in the quadrants 0,3,4 or 7
-  register int quad0347 = abs(x)<abs(y);
-  
-  if (x<0) { // quad 4567
-    if (y<0) { // quad 45
-      return(quad0347 ? 4 : 5);
-    } else { // quad 67
-      return(quad0347 ? 7 : 6);
-    }
-  } else {
-    if (y<0) { // quad 23
-      return(quad0347 ? 3 : 2);
-    } else { // quad 01
-      return(quad0347 ? 0 : 1);
-    }
-  }  
-}
-#endif
-
 // Will trace the configured arc until the target is reached. 
 void execute_arc()
 {
-  uint32_t start_x = state.arc.x;
-  uint32_t start_y = state.arc.y;
+  uint32_t start_x = mc.arc.x;
+  uint32_t start_y = mc.arc.y;
   int dx, dy; // Trace directions
+  int8_t direction[3];
 
-  // state.mode is set to 0 (MC_MODE_AT_REST) when target is reached
-  while(state.arc.incomplete)
+  // mc.mode is set to 0 (MC_MODE_AT_REST) when target is reached
+  while(mc.arc.incomplete)
   {
-#ifdef UNROLLED_ARC_LOOP
-    // Unrolling the arc code is fast, but costs about 830 bytes of extra code space.
-    int q = quadrant(state.arc.x, state.arc.y);
-    if (state.arc.angular_direction) {
-      switch (q) {
-        case 0: 
-        map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.arc.incomplete && (state.arc.x>state.arc.y)) { step_arc_along_x(1,-1); }
-        case 1: 
-        map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.arc.incomplete && (state.arc.y>0)) { step_arc_along_y(1,-1); }
-        case 2: 
-        map_local_arc_directions_to_stepper_directions(-1,-1);
-        while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_y(-1,-1); }
-        case 3: 
-        map_local_arc_directions_to_stepper_directions(-1,-1);
-        while(state.arc.incomplete && (state.arc.x>0)) { step_arc_along_x(-1,-1); }
-        case 4: 
-        map_local_arc_directions_to_stepper_directions(-1,1);
-        while(state.arc.incomplete && (state.arc.y<state.arc.x)) { step_arc_along_x(-1,1); }
-        case 5: 
-        map_local_arc_directions_to_stepper_directions(-1,1);
-        while(state.arc.incomplete && (state.arc.y<0)) { step_arc_along_y(-1,1); }
-        case 6: 
-        map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_y(1,1); }
-        case 7: 
-        map_local_arc_directions_to_stepper_directions(1,1);
-        while(state.arc.incomplete && (state.arc.x<0)) { step_arc_along_x(1,1); }
-      }
-    } else {
-      switch (q) {
-        case 7:
-        map_local_arc_directions_to_stepper_directions(-1,-1);
-        while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_x(-1,-1); }
-        case 6:
-        map_local_arc_directions_to_stepper_directions(-1,-1);
-        while(state.arc.incomplete && (state.arc.y>0))  { step_arc_along_y(-1,-1); }
-        case 5:
-        map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.arc.incomplete && (state.arc.y>state.arc.x))  { step_arc_along_y(1,-1); }
-        case 4:
-        map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.arc.incomplete && (state.arc.x<0))  { step_arc_along_x(1,-1); }
-        case 3:
-        map_local_arc_directions_to_stepper_directions(1,1);
-        while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_x(1,1); }      
-        case 2:
-        map_local_arc_directions_to_stepper_directions(1,1);
-        while(state.arc.incomplete && (state.arc.y<0))  { step_arc_along_y(1,1); }      
-        case 1:
-        map_local_arc_directions_to_stepper_directions(-1,1);
-        while(state.arc.incomplete && (state.arc.y<state.arc.x))  { step_arc_along_y(-1,1); }
-        case 0:
-        map_local_arc_directions_to_stepper_directions(-1,1);
-        while(state.arc.incomplete && (state.arc.x>0))  { step_arc_along_x(-1,1); }
-      }    
-    }
-#else 
-    dx = (state.arc.y!=0) ?  sign(state.arc.y) * state.arc.angular_direction : -sign(state.arc.x);
-    dy = (state.arc.x!=0) ? -sign(state.arc.x) * state.arc.angular_direction : -sign(state.arc.y);
-    if (fabs(state.arc.x)<fabs(state.arc.y)) {
+    dx = (mc.arc.y!=0) ?  signof(mc.arc.y) * mc.arc.angular_direction : -signof(mc.arc.x);
+    dy = (mc.arc.x!=0) ? -signof(mc.arc.x) * mc.arc.angular_direction : -signof(mc.arc.y);
+
+    // Take dx and dy which are local to the arc being generated and map them on to the 
+    // selected tool-space-axes for the current arc.
+    direction[mc.arc.axis_x] = dx;
+    direction[mc.arc.axis_y] = dy;
+    set_stepper_directions(direction);
+
+    if (abs(mc.arc.x)<abs(mc.arc.y)) {
       step_arc_along_x(dx,dy);    
-    } else {
+    } else {      
       step_arc_along_y(dx,dy);    
     }
-#endif    
   }
+  
   // Update the tool position to the new actual position
-  state.position[state.arc.axis_x] += state.arc.x-start_x;
-  state.position[state.arc.axis_y] += state.arc.y-start_y;
+  mc.position[mc.arc.axis_x] += mc.arc.x-start_x;
+  mc.position[mc.arc.axis_y] += mc.arc.y-start_y;
   // Because of rounding errors we might be off by a step or two. Adjust for this
     // To be implemented
   //void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
-    
+  mc.mode = MC_MODE_AT_REST;  
 }
 
 void mc_go_home()
 {
-  state.mode = MC_MODE_HOME;
+  mc.mode = MC_MODE_HOME;
 }
 
 void execute_go_home() 
 {
   st_go_home();
   st_synchronize();
-  clear_vector(state.position); // By definition this is location [0, 0, 0]
-  state.mode = MC_MODE_AT_REST;
+  clear_vector(mc.position); // By definition this is location [0, 0, 0]
+  mc.mode = MC_MODE_AT_REST;
 }
 
 void mc_execute() {
-  st_set_pace(state.pace);
-  while(state.mode) {
-    switch(state.mode) {
+  st_set_pace(mc.pace);
+  printByte('~');
+  while(mc.mode) { // Loop because one task might start another task
+    switch(mc.mode) {
       case MC_MODE_AT_REST: break;
-      case MC_MODE_DWELL: st_synchronize(); _delay_ms(state.dwell_milliseconds); state.mode = MC_MODE_AT_REST; break;
+      case MC_MODE_DWELL: st_synchronize(); _delay_ms(mc.dwell_milliseconds); mc.mode = MC_MODE_AT_REST; break;
       case MC_MODE_LINEAR: execute_linear_motion(); break;
       case MC_MODE_ARC: execute_arc(); break;
       case MC_MODE_HOME: execute_go_home(); break;
@@ -414,7 +338,7 @@ void mc_execute() {
 
 int mc_status() 
 {
-  return(state.mode);
+  return(mc.mode);
 }
 
 // Set the direction pins for the stepper motors according to the provided vector. 
@@ -427,11 +351,10 @@ void set_stepper_directions(int8_t *direction)
      way we can generate the whole direction bit-mask without doing any comparisions
      or branching. Fast and compact, yet practically unreadable. Sorry sorry sorry.
   */
-  direction_bits = ~(
+  mc.direction_bits = (
     ((direction[X_AXIS]&0x80)>>(7-X_DIRECTION_BIT)) |
     ((direction[Y_AXIS]&0x80)>>(7-Y_DIRECTION_BIT)) |
-    ((direction[Z_AXIS]&0x80)>>(7-Z_DIRECTION_BIT))
-  );
+    ((direction[Z_AXIS]&0x80)>>(7-Z_DIRECTION_BIT)));
 }
 
 // Step enabled steppers. Enabled should be an array of three bytes. Each byte represent one
@@ -439,16 +362,17 @@ void set_stepper_directions(int8_t *direction)
 // 1, and the rest to 0. 
 inline void step_steppers(uint8_t *enabled) 
 {
-  st_buffer_step(direction_bits | enabled[X_AXIS]<<X_STEP_BIT | enabled[Y_AXIS]<<Y_STEP_BIT | enabled[Z_AXIS]<<Z_STEP_BIT);
+  st_buffer_step(mc.direction_bits | (enabled[X_AXIS]<<X_STEP_BIT) | 
+    (enabled[Y_AXIS]<<Y_STEP_BIT) | (enabled[Z_AXIS]<<Z_STEP_BIT));
 }
 
 // Step only one motor
 inline void step_axis(uint8_t axis) 
 {
   switch (axis) {
-    case X_AXIS: st_buffer_step(direction_bits | (1<<X_STEP_BIT)); break;
-    case Y_AXIS: st_buffer_step(direction_bits | (1<<Y_STEP_BIT)); break;
-    case Z_AXIS: st_buffer_step(direction_bits | (1<<Z_STEP_BIT)); break;
+    case X_AXIS: st_buffer_step(mc.direction_bits | (1<<X_STEP_BIT)); break;
+    case Y_AXIS: st_buffer_step(mc.direction_bits | (1<<Y_STEP_BIT)); break;
+    case Z_AXIS: st_buffer_step(mc.direction_bits | (1<<Z_STEP_BIT)); break;
   }
 }
 
@@ -457,3 +381,4 @@ void mc_wait()
 {
   st_synchronize();
 }
+
diff --git a/motion_control.h b/motion_control.h
index e7bf6db..2867451 100644
--- a/motion_control.h
+++ b/motion_control.h
@@ -61,4 +61,6 @@ void mc_wait();
 // result < 0: the system is in an error state.
 int mc_status();
 
+void printCurrentPosition();
+
 #endif
diff --git a/nuts_bolts.h b/nuts_bolts.h
index 182fc27..91eabbc 100644
--- a/nuts_bolts.h
+++ b/nuts_bolts.h
@@ -30,7 +30,7 @@
 #define true 1
 
 // Decide the sign of a value
-#define sign(a) (a>0 ? 1 : ((a<0) ? -1 : 0))
+#define signof(a) ((a>0) ? 1 : ((a<0) ? -1 : 0))
 
 #define clear_vector(a) memset(a, 0, sizeof(a))
 
diff --git a/serial_protocol.c b/serial_protocol.c
index 2a6894e..cd8b090 100644
--- a/serial_protocol.c
+++ b/serial_protocol.c
@@ -33,6 +33,7 @@ uint8_t line_counter;
 
 void prompt() {
   printString(PROMPT);
+  line_counter = 0;
 }
 
 void print_result() {
@@ -42,7 +43,7 @@ void print_result() {
   uint32_t line_number;
   int i; // loop variable
   gc_get_status(position, &status_code, &inches_mode, &line_number);
-  printString("[ ");  
+  printString("\r\n[ ");  
   for(i=X_AXIS; i<=Z_AXIS; i++) {
     printInteger(trunc(position[i]*100));
     printByte(' ');
@@ -52,11 +53,12 @@ void print_result() {
   printInteger(line_number);
   printByte(':');
   switch(status_code) {
-    case GCSTATUS_OK: printString("0 OK\n"); break;
-    case GCSTATUS_BAD_NUMBER_FORMAT: printString("1 Bad number format\n");
-    case GCSTATUS_EXPECTED_COMMAND_LETTER: printString("2 Expected command letter\n"); break;
-    case GCSTATUS_UNSUPPORTED_STATEMENT: printString("3 Unsupported statement\n"); break;
-    case GCSTATUS_MOTION_CONTROL_ERROR: printString("4 Motion control error\n"); break;
+    case GCSTATUS_OK: printString("0 OK\r\n"); break;
+    case GCSTATUS_BAD_NUMBER_FORMAT: printString("1 Bad number format\r\n"); break;
+    case GCSTATUS_EXPECTED_COMMAND_LETTER: printString("2 Expected command letter\r\n"); break;
+    case GCSTATUS_UNSUPPORTED_STATEMENT: printString("3 Unsupported statement\r\n"); break;
+    case GCSTATUS_MOTION_CONTROL_ERROR: printString("4 Motion control error\r\n"); break;
+    case GCSTATUS_FLOATING_POINT_ERROR: printString("5 Floating point error\r\n"); break;
   }
 }
 
@@ -64,9 +66,9 @@ void sp_init()
 {
   beginSerial(BAUD_RATE);
   
-  printString("Grbl ");
+  printString("\r\nGrbl ");
   printString(VERSION);
-  printByte('\n');  
+  printByte('\r');  
   prompt();
 }
 
@@ -75,14 +77,20 @@ void sp_process()
   char c;
   while((c = serialRead()) != -1) 
   {
-    if(c == '\n') {
+    //printByte(c); // Echo
+    if(c == '\r') {
       line[line_counter] = 0;
+      //printByte(EXECUTION_MARKER);
       gc_execute_line(line);
       line_counter = 0;
       print_result();
       prompt();
+    } else if (c == ' ' || c == '\t') {
+      // Throw away whitepace
+    } else if (c >= 'a' && c <= 'z') {
+      line[line_counter++] = c-'a'+'A';
     } else {
-      line[line_counter] = c;
+      line[line_counter++] = c;
     }
   }
 }
diff --git a/serial_protocol.h b/serial_protocol.h
index 00d3e0d..6420fe8 100644
--- a/serial_protocol.h
+++ b/serial_protocol.h
@@ -20,7 +20,10 @@
 #ifndef serial_h
 #define serial_h
 
-#define PROMPT ">>>"
+// A string to let the client know we are ready for a new command
+#define PROMPT "\r\n>>>"
+// A character to acknowledge that the execution has started
+#define EXECUTION_MARKER '~'
 
 void sp_init();
 void sp_process();
diff --git a/stepper.c b/stepper.c
index 0e4fb9b..c5fedab 100644
--- a/stepper.c
+++ b/stepper.c
@@ -19,13 +19,16 @@
 */
 
 /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
-   and Philipp Tiefenbacher. The circle buffer implementation by the wiring_serial library by David A. Mellis */
+   and Philipp Tiefenbacher. The circle buffer implementation gleaned from the wiring_serial library 
+   by David A. Mellis */
 
 #include "stepper.h"
 #include "config.h"
 #include "nuts_bolts.h"
 #include <avr/interrupt.h>
 
+#include "wiring_serial.h"
+
 #define TICKS_PER_MICROSECOND F_CPU/1000000
 #define STEP_BUFFER_SIZE 100
 
@@ -34,37 +37,38 @@ volatile int step_buffer_head = 0;
 volatile int step_buffer_tail = 0;
 
 uint8_t stepper_mode = STEPPER_MODE_STOPPED;
+uint8_t echo_steps = true;
 
 // This timer interrupt is executed at the pace set with set_pace. It pops one instruction from
 // the step_buffer, executes it. Then it starts timer2 in order to reset the motor port after
 // five microseconds.
 SIGNAL(SIG_OUTPUT_COMPARE1A)
 {
-  if (step_buffer_head != step_buffer_tail) {
-    // Set the stepper port according to the instructions
-    MOTORS_PORT = (MOTORS_PORT & ~MOTORS_MASK) | step_buffer[step_buffer_tail];
-    // Reset and start timer 2 which will reset the motor port after 5 microsecond
-    TCNT2 = 0; // reset counter
-    OCR2A = 5*TICKS_PER_MICROSECOND; // set the time
-    TIMSK2 |= OCIE2A; // enable interrupt
-    // move the step buffer tail to the next instruction
-    step_buffer_tail = (step_buffer_tail + 1) % STEP_BUFFER_SIZE;
-	}
+    if (step_buffer_head != step_buffer_tail) {
+      // Set the stepper port according to the instructions
+      STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | step_buffer[step_buffer_tail];
+      // Reset and start timer 2 which will reset the motor port after 5 microsecond
+      // TCNT2 = 0; // reset counter
+      // OCR2A = 5*TICKS_PER_MICROSECOND; // set the time
+      // TIMSK2 |= (1<<OCIE2A); // enable interrupt
+      // move the step buffer tail to the next instruction
+      step_buffer_tail = (step_buffer_tail + 1) % STEP_BUFFER_SIZE;
+  }
 }
 
 // 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 (5us) completing one step cycle.
 SIGNAL(SIG_OUTPUT_COMPARE2A)
 {
-  MOTORS_PORT = MOTORS_PORT & ~MOTORS_MASK; // reset stepper pins
-  TIMSK2 &= ~OCIE2A; // disable this timer interrupt until next time
+  STEPPING_PORT = STEPPING_PORT & ~STEPPING_MASK; // reset stepper pins
+  TIMSK2 &= ~(1<<OCIE2A); // disable this timer interrupt until next time
 }
 
 // Initialize and start the stepper motor subsystem
 void st_init()
 {
 	// Configure directions of interface pins
-  MOTORS_DDR   |= MOTORS_MASK;
+  STEPPING_DDR   |= STEPPING_MASK;
   LIMIT_DDR &= ~(LIMIT_MASK);
   STEPPERS_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
 
@@ -92,13 +96,18 @@ void st_init()
 
 void st_buffer_step(uint8_t motor_port_bits)
 {
+  if (echo_steps) {
+    printByte('!'+motor_port_bits);
+//    printIntegerInBase(motor_port_bits,2);printByte('\r');printByte('\n');
+  }
+
 	int i = (step_buffer_head + 1) % STEP_BUFFER_SIZE;
 	
 	// If the buffer is full: good! That means we are well ahead of the robot. 
 	// Nap until there is room for more steps.
 	while(step_buffer_tail == i) { sleep_mode(); }
 	
-  step_buffer[step_buffer_head] = motor_port_bits;
+//  step_buffer[step_buffer_head] = motor_port_bits;
   step_buffer_head = i;
 }
 
@@ -140,6 +149,12 @@ inline void st_stop()
 
 void st_set_pace(uint32_t microseconds)
 {
+  printString("pace = ");
+  printInteger(microseconds);
+  printString("\n\r");
+  if (microseconds <= 500) {
+    microseconds = 500;
+  }
   uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;
   uint16_t ceiling;
   uint16_t prescaler;
@@ -163,6 +178,7 @@ void st_set_pace(uint32_t microseconds)
 		ceiling = 0xffff;
     prescaler = 4;
 	}
+		
 	// Set prescaler
   TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
   // Set ceiling
@@ -226,3 +242,8 @@ void st_go_home()
 {
   // Todo: Perform the homing cycle
 }
+
+void st_set_echo(int value)
+{
+  echo_steps = value;
+}
diff --git a/stepper.h b/stepper.h
index 6ab129b..44c87d0 100644
--- a/stepper.h
+++ b/stepper.h
@@ -53,4 +53,7 @@ inline void st_stop();
 // Execute the homing cycle
 void st_go_home();
 
+// Echo steps to serial port? (true/false)
+void st_set_echo(int value);
+
 #endif
diff --git a/testbench/echotest.rb b/testbench/echotest.rb
new file mode 100644
index 0000000..e1a80fa
--- /dev/null
+++ b/testbench/echotest.rb
@@ -0,0 +1,225 @@
+require 'pp'
+require 'rubygems'
+require 'serialport'
+require 'png'
+
+# Requires the gems serialport and png
+# 
+# $ gem install ruby-serialport
+# $ gem install png
+
+
+@sp = SerialPort.new '/dev/tty.usbserial-A4001o6L', 19200
+
+@canvas = PNG::Canvas.new(500,500)
+@x = 0
+@y = 0
+@ready_for_command = true
+@last_byte = Time.now
+
+@input = ''
+
+@step_count = 0
+
+Thread.new do
+  step_mode = false
+  @sp.each_byte do |byte| 
+    #puts byte.chr
+       
+    @last_byte = Time.now
+    @input << byte.chr
+    if byte == 126 # ('~')
+      step_mode = true          
+      @input << '[step mode on]'
+    else
+      if step_mode 
+        @step_count += 1
+        if byte == 10
+          @input << '[step mode off]'
+          step_mode = false
+        else
+          step = byte-33
+          puts @step_count
+          if (step & 1) != 0
+            if (step & 8) != 0
+              puts "x-1"
+              @x+=1
+            else
+              puts "x+1"
+              @x-=1
+            end
+          end
+          if (step & 2) != 0
+            if (step & 16) != 0
+              puts "y-1"
+              @y+=1
+            else
+              puts "y+1"
+              @y-=1
+            end
+          end
+        end
+      else
+        if byte == 62 # '>'
+          @ready_for_command = true
+        end
+      end
+    end
+    @canvas[250-@x,250-@y] = PNG::Color::Black    
+  end
+end
+
+def send_command(command)
+  while !@ready_for_command do
+    sleep(1)
+    puts "Processing ..."
+    if (Time.now-@last_byte > 5) 
+      return
+    end
+  end
+  sleep(0.5)
+  puts "Sent: #{command}"
+  @input << "[sent #{command}]"
+  @sp.write("#{command}\r")
+  @ready_for_command = false
+end
+
+
+blocks = <<-END.split("\n").map{|s|s.strip}
+N10 G00 Z100.000  G53
+N15 G00 X38.105 Y71.468  G53
+N20 Z2.0 F200  G53
+N25 G01 Z-0.2 G53
+N30 X37.177 Y74.406 G53
+N35 X37.779 Y74.698 G53
+N40 X41.077 Y76.347 G53
+N45 X42.177 Y73.719 G53
+N50 X40.304 Y72.173 G53
+N55 X39.548 Y73.873 G53
+N60 X38.638 Y73.152 G53
+N65 X39.153 Y71.915 G53
+N70 X38.105 Y71.468 G53
+N75 G00 Z2.0 G53
+N80 X37.266 Y75.042  G53
+N85 G01 Z-0.2 G53
+N90 X37.862 Y79.198 G53
+N95 G00 Z2.0 G53
+N100 X37.684 Y77.960  G53
+N105 G01 Z-0.2 G53
+N110 X42.718 Y77.239 G53
+N115 G00 Z2.0 G53
+N120 X42.895 Y78.476  G53
+N125 G01 Z-0.2 G53
+N130 X42.300 Y74.320 G53
+N135 G00 Z2.0 G53
+N140 X43.975 Y77.690  G53
+N145 G01 Z-0.2 G53
+N150 X43.425 Y77.769 G53
+N155 G00 Z2.0 G53
+N160 X44.604 Y78.161  G53
+N165 G01 Z-0.2 G53
+N170 G02 X43.975 Y77.690 I-0.550 J0.079 G53
+N175 G00 Z2.0 G53
+N180 X44.643 Y78.436  G53
+N185 G01 Z-0.2 G53
+N190 X44.604 Y78.161 G53
+N195 G00 Z2.0 G53
+N200 X44.172 Y79.065  G53
+N205 G01 Z-0.2 G53
+N210 G02 X44.643 Y78.436 I-0.079 J-0.550 G53
+N215 G00 Z2.0 G53
+N220 X43.622 Y79.144  G53
+N225 G01 Z-0.2 G53
+N230 X44.172 Y79.065 G53
+N235 G00 Z2.0 G53
+N240 X43.780 Y80.243  G53
+N245 G01 Z-0.2 G53
+N250 X43.622 Y79.144 G53
+N255 G00 Z2.0 G53
+N260 X44.880 Y80.086  G53
+N265 G01 Z-0.2 G53
+N270 X43.780 Y80.243 G53
+N275 G00 Z2.0 G53
+N280 X46.488 Y79.154  G53
+N285 G01 Z-0.2 G53
+N290 G02 X46.808 Y79.669 I1.265 J-0.427 G53
+N295 G02 X47.312 Y79.597 I0.229 J-0.198 G53
+N300 G02 X47.474 Y79.012 I-1.172 J-0.639 G53
+N305 G02 X47.452 Y78.454 I-2.606 J-0.179 G53
+N310 G02 X47.316 Y77.912 I-2.587 J0.362 G53
+N315 G02 X46.997 Y77.397 I-1.265 J0.427 G53
+N320 G02 X46.493 Y77.469 I-0.229 J0.198 G53
+N325 G02 X46.331 Y78.054 I1.172 J0.639 G53
+N330 G02 X46.352 Y78.612 I2.606 J0.179 G53
+N335 G02 X46.489 Y79.154 I2.587 J-0.362 G53
+N340 G00 Z2.0 G53
+N345 X45.370 Y77.630  G53
+N350 G01 Z-0.2 G53
+N355 X45.350 Y77.493 G53
+N360 G00 Z2.0 G53
+N365 X48.179 Y77.649  G53
+N370 G01 Z-0.2 G53
+N375 X48.218 Y77.924 G53
+N380 G02 X49.318 Y77.766 I0.550 J-0.079 G53
+N385 G01 X49.278 Y77.491 G53
+N390 G02 X48.179 Y77.649 I-0.550 J0.079 G53
+N395 G00 Z2.0 G53
+N400 X48.435 Y78.945  G53
+N405 G01 Z-0.2 G53
+N410 G03 X49.397 Y78.807 I0.481 J-0.069 G53
+N415 G01 X49.417 Y78.944 G53
+N420 G03 X48.454 Y79.082 I-0.481 J0.069 G53
+N425 G01 X48.435 Y78.945 G53
+N430 G00 Z2.0 G53
+N435 X50.379 Y79.297  G53
+N440 G01 Z-0.2 G53
+N445 X51.479 Y79.140 G53
+N450 G00 Z2.0 G53
+N455 X50.222 Y78.198  G53
+N460 G01 Z-0.2 G53
+N465 X50.379 Y79.297 G53
+N470 G00 Z2.0 G53
+N475 X50.772 Y78.119  G53
+N480 G01 Z-0.2 G53
+N485 X50.222 Y78.198 G53
+N490 G00 Z2.0 G53
+N495 X51.243 Y77.490  G53
+N500 G01 Z-0.2 G53
+N505 G03 X50.772 Y78.119 I-0.550 J0.079 G53
+N510 G00 Z2.0 G53
+N515 X51.203 Y77.215  G53
+N520 G01 Z-0.2 G53
+N525 X51.243 Y77.490 G53
+N530 G00 Z2.0 G53
+N535 X50.574 Y76.744  G53
+N540 G01 Z-0.2 G53
+N545 G03 X51.203 Y77.215 I0.079 J0.550 G53
+N550 G00 Z2.0 G53
+N555 X50.025 Y76.823  G53
+N560 G01 Z-0.2 G53
+N565 X50.574 Y76.744 G53
+N570 G00 Z2.0 G53
+N575 X53.049 Y76.389  G53
+N580 G01 Z-0.2 G53
+N585 X51.949 Y76.547 G53
+N590 X53.274 Y78.222 G53
+N595 G03 X53.327 Y78.454 I-0.218 J0.172 G53
+N600 G03 X52.262 Y78.607 I-0.552 J-0.061 G53
+N605 G00 Z2.0 G53
+N610 M30 G53
+END
+
+blocks.each do |block|
+  send_command(block)
+  if (Time.now-@last_byte > 5) 
+    puts "Bailing, cause somethin' went wrong"
+    break
+  end
+end
+
+sleep(3);
+
+PNG.new(@canvas).save("gcodetest.png")
+
+puts @input
+`open gcodetest.png`
diff --git a/testbench/gcodetest.png b/testbench/gcodetest.png
new file mode 100644
index 0000000..0a8e051
Binary files /dev/null and b/testbench/gcodetest.png differ