   Serial.print("z: "); Serial.println(getPositionZ());
 #endif
   
-  if (isMotionDoneX()) {
-    moveToX(xTarget == 0 ? 4000 : 0);
-  }
-  
-  if (isMotionDoneY()) {
-    moveToY(yTarget == 0 ? 3000 : 0);
-  }
-  
-  if (isMotionDoneZ()) {
-    moveToZ(zTarget == 0 ? 2000 : 0);
-  }
-}
+  if (!homed && !isHomeX() && isMotionDoneX()) {
+    moveToX(getPositionX() - 5); // NB: this can wrap around! Should check.
+  }
+  
+  if (!homed && !isHomeY() && isMotionDoneY()) {
+    moveToY(getPositionY() - 5); 
+  }
+  
+  if (!homed && !isHomeZ() && isMotionDoneZ()) {
+    moveToZ(getPositionZ() - 5); 
+  }
+  
+  if (isHomeX() && isHomeY() && isHomeZ()) {
+    homed = true;
+    xPosition = 0;
+    yPosition = 0;
+    zPosition = 0;
+  }
+  
+  if (homed) {
+    moveToX(SAFE_LIMIT / 2);
+    moveToY(SAFE_LIMIT / 2);
+    moveToZ(SAFE_LIMIT / 2);
+  }
+}

boolean isHomeZ() {
  return AT_Z_MIN();
}

void loop() {
  

    zTicksRemaining = accelTab[zAccelStep];
    
    if (zPosition < zTarget) {
      FORWARD_Z();
      if (zPosition < SAFE_LIMIT) {
        ++zPosition;
        STEP_Z();
      }
    } else if (zPosition > zTarget) {
      BACKWARD_Z();
      if (!AT_Z_MIN()) {
        --zPosition;
        STEP_Z();
