diff --git a/config.h b/config.h
index 1bbc7ea..decf61d 100644
--- a/config.h
+++ b/config.h
@@ -50,7 +50,6 @@
 #define Y_DIRECTION_BIT            4
 #define Z_DIRECTION_BIT            5
 
-
 #define LIMIT_DDR      DDRD
 #define LIMIT_PORT     PORTD
 #define X_LIMIT_BIT          3
@@ -65,6 +64,8 @@
 #define SPINDLE_DIRECTION_PORT PORTD
 #define SPINDLE_DIRECTION_BIT 7
 
+#define MM_PER_ARC_SEGMENT 0.1
+
 #define BAUD_RATE 9600
 
 #define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT))
diff --git a/gcode.c b/gcode.c
index ae11358..d15ff20 100644
--- a/gcode.c
+++ b/gcode.c
@@ -373,6 +373,9 @@ uint8_t gc_execute_line(char *line) {
       // Trace the arc
       mc_arc(theta_start, angular_travel, radius, depth, 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);
+      // 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;
     }    
   }
diff --git a/motion_control.c b/motion_control.c
index c702b8c..90fe6b2 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -105,7 +105,25 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
 // 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, double linear_travel, int axis_1, int axis_2, 
   int axis_linear, double feed_rate, int invert_feed_rate)
-{  
+{
+  double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel));
+  if (millimeters_of_travel == 0.0) { return; }
+  uint16_t segments = ceil(millimeters_of_travel/MM_PER_ARC_SEGMENT);
+  if (invert_feed_rate) { feed_rate *= segments; }
+  double theta_per_segment = angular_travel/segments;
+  double linear_per_segment = linear_travel/segments;
+  double center_x = (position[axis_1]/X_STEPS_PER_MM)-sin(theta)*radius;
+  double center_y = (position[axis_2]/Y_STEPS_PER_MM)-cos(theta)*radius;
+  double target[3];
+  int i;
+  target[axis_linear] = position[axis_linear];
+  for (i=0; i<=segments; i++) {
+    target[axis_linear] += linear_per_segment;
+    theta += theta_per_segment;
+    target[axis_1] = center_x+sin(theta)*radius;
+    target[axis_2] = center_y+cos(theta)*radius;
+    mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
+  }
 }
 
 void mc_go_home()
diff --git a/stepper.c b/stepper.c
index a860aa3..ff41380 100644
--- a/stepper.c
+++ b/stepper.c
@@ -56,6 +56,8 @@ uint8_t stepper_mode = STEPPER_MODE_STOPPED;
 
 void config_pace_timer(uint32_t microseconds);
 
+// Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in 
+// steps. Microseconds specify how many microseconds the move should take to perform.
 void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds) {
   // Buffer nothing unless stepping subsystem is running
   if (stepper_mode != STEPPER_MODE_RUNNING) { return; }
diff --git a/stepper.h b/stepper.h
index 6c859d8..2034ae4 100644
--- a/stepper.h
+++ b/stepper.h
@@ -35,7 +35,8 @@ void st_init();
 // Returns a bitmask with the stepper bit for the given axis set
 uint8_t st_bit_for_stepper(int axis);
 
-// Buffer a new line segment (might block until there is room in the buffer)
+// Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in 
+// steps. Microseconds specify how many microseconds the move should take to perform.
 void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t rate);
 
 // Block until all buffered steps are executed