     
     if (yPosition < yTarget) {
       SET_Y_DIR();
-      if (yPosition < SAFE_LIMIT) ++yPosition;
+      if (yPosition < SAFE_LIMIT) {
+        ++yPosition;
+        STEP_Y();
+      }
     } else if (yPosition > yTarget) {
       CLR_Y_DIR();
-      if (!AT_Y_MIN()) --yPosition;
+      if (!AT_Y_MIN()) {
+        --yPosition;
+        STEP_Y();
+      }
     }  
   }
   

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

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

    
    if (zPosition < zTarget) {
      SET_Z_DIR();
      if (zPosition < SAFE_LIMIT) {
        ++zPosition;
        STEP_Z();
      }
    } else if (zPosition > zTarget) {
      CLR_Z_DIR();
      if (!AT_Z_MIN()) {
        --zPosition;
        STEP_Z();
      }
    }  
  } 
}

// Note: target updates that cause an instant change of direction
// will not accelerate or decelerate. Wait for motion to finish before doing this.

void moveToX(uint16_t target) {
  if (target > SAFE_LIMIT) target = SAFE_LIMIT;
