diff --git a/Makefile b/Makefile
index ae641fc..65a33c2 100644
--- a/Makefile
+++ b/Makefile
@@ -31,7 +31,7 @@ DEVICE     = atmega328p
 CLOCK      = 16000000
 PROGRAMMER = -c avrisp2 -P usb
 OBJECTS    = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o protocol.o stepper.o \
-             eeprom.o settings.o planner.o nuts_bolts.o
+             eeprom.o settings.o planner.o nuts_bolts.o limits.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: 
diff --git a/config.h b/config.h
index 1f2e84b..3a82e46 100644
--- a/config.h
+++ b/config.h
@@ -40,7 +40,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
diff --git a/gcode.c b/gcode.c
index c5e656e..48dc64a 100644
--- a/gcode.c
+++ b/gcode.c
@@ -231,7 +231,7 @@ uint8_t gc_execute_line(char *line) {
   
   // 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(gc.position); break;
     case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break;
     case NEXT_ACTION_DEFAULT: 
     switch (gc.motion_mode) {
diff --git a/limits.c b/limits.c
new file mode 100644
index 0000000..6444c8d
--- /dev/null
+++ b/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));
+}
diff --git a/limits.h b/limits.h
new file mode 100644
index 0000000..847c667
--- /dev/null
+++ b/limits.h
@@ -0,0 +1,30 @@
+/*
+  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/>.
+*/
+
+#ifndef limits_h
+#define limits_h 
+
+// initialize the limits module
+void limits_init();
+
+// perform the homing cycle
+void limits_go_home();
+
+#endif
\ No newline at end of file
diff --git a/main.c b/main.c
index 5179c6b..a6847d3 100644
--- a/main.c
+++ b/main.c
@@ -27,6 +27,7 @@
 #include "motion_control.h"
 #include "gcode.h"
 #include "protocol.h"
+#include "limits.h"
 
 #include "settings.h"
 #include "wiring_serial.h"
@@ -42,7 +43,8 @@ int main(void)
   plan_init();      
   st_init();        
   spindle_init();   
-  gc_init();        
+  gc_init();
+  limits_init();  
                     
   for(;;){
     sleep_mode(); // Wait for it ...
diff --git a/nuts_bolts.h b/nuts_bolts.h
index bc0b892..e5a2ea8 100644
--- a/nuts_bolts.h
+++ b/nuts_bolts.h
@@ -22,6 +22,7 @@
 #define nuts_bolts_h
 #include <string.h>
 #include <stdint.h>
+#include <stdbool.h>
 
 #define false 0
 #define true 1
diff --git a/planner.c b/planner.c
index 45a9526..bcc2a91 100644
--- a/planner.c
+++ b/planner.c
@@ -386,3 +386,10 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
   if (acceleration_manager_enabled) { planner_recalculate(); }  
   st_wake_up();
 }
+
+// Reset the position vector
+void plan_set_current_position(double x, double y, double z) {
+  position[X_AXIS] = x;
+  position[Y_AXIS] = y;
+  position[Z_AXIS] = z;
+}
diff --git a/planner.h b/planner.h
index ab622d5..c39e8fc 100644
--- a/planner.h
+++ b/planner.h
@@ -70,4 +70,7 @@ void plan_set_acceleration_manager_enabled(int enabled);
 // Is acceleration-management currently enabled?
 int plan_is_acceleration_manager_enabled();
 
+// Reset the position vector
+void plan_set_current_position(double x, double y, double z); 
+
 #endif
\ No newline at end of file
diff --git a/stepper.c b/stepper.c
index 640ab8c..9eb5d7d 100644
--- a/stepper.c
+++ b/stepper.c
@@ -31,13 +31,12 @@
 #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)
@@ -212,7 +211,6 @@ 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_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
   
 	// waveform generation = 0100 = CTC
@@ -293,5 +291,6 @@ static void set_step_events_per_minute(uint32_t steps_per_minute) {
 
 void st_go_home()
 {
-  // Todo: Perform the homing cycle
+  limits_go_home();  
+  plan_set_current_position(0,0,0);
 }
diff --git a/stepper.h b/stepper.h
index 66cc77d..5d5758c 100644
--- a/stepper.h
+++ b/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();