 
 void loop() {
   
-  if (xPosition == xTarget) {
-    moveToX(xTarget == 0 ? 10000 : 0);
-  }
-  
-  if (yPosition == yTarget) {
-    moveToY(yTarget == 0 ? 7500 : 0);
-  }
-  
-  if (zPosition == zTarget) {
-    moveToZ(zTarget == 0 ? 5000 : 0);
-  }
-}
+#ifdef DEBUG
+  Serial.print("x: "); Serial.println(getPositionX());
+  Serial.print("y: "); Serial.println(getPositionY());
+  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);
+  }
+}

#define ACCEL_STEPS 75
const uint8_t accelTab[] = {71,29,22,19,17,15,14,13,12,11,11,10,10,10,9,9,9,9,8,8,8,8,8,7,7,7,7,7,7,6,6,6,6,6,6,6,5,5,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,1};

volatile uint8_t xAccelStep = 0; // the acceleration step we are on, index into accelTab[]
volatile uint16_t xTicksRemaining = 1; // number of ticks before next step is issued
volatile uint8_t yAccelStep = 0;
volatile uint16_t yTicksRemaining = 1; 
volatile uint8_t zAccelStep = 0; 
volatile uint16_t zTicksRemaining = 1;

ISR(TIMER0_COMPA_vect) {
  

#define CLR_Y_STEP() PORTF &= ~_BV(PF6)
#define SET_Z_STEP() PORTL |= _BV(PL3)
#define CLR_Z_STEP() PORTL &= ~_BV(PL3)

#define STEP_X() do { SET_X_STEP(); CLR_X_STEP(); } while (0)
#define STEP_Y() do { SET_Y_STEP(); CLR_Y_STEP(); } while (0)
#define STEP_Z() do { SET_Z_STEP(); CLR_Z_STEP(); } while (0)

#define SET_X_DIR() PORTF |= _BV(PF1)
#define CLR_X_DIR() PORTF &= ~_BV(PF1)
