 				break;
 		}
 		
-		x *= mmToStep * scale;
-		y *= mmToStep * scale;
-		
-	}
-	
+		x *= scale;
+		y *= scale;
+		
+	}
+	
+	x *= mmToStep;
+	y *= mmToStep;
 	if ( current_pos.x == x && current_pos.y == y) {
 		// it's a useless move to!
 		return;

				break;
		}
		
		x *= mmToStep * scale;
		y *= mmToStep * scale;
		
	}
	
	if (current_pos.z != z) {
		moveTo(current_pos.x, current_pos.y, z);

				break;
		}
		
		x *= scale;
		y *= scale;
		
	}
	
	x *= mmToStep;
	y *= mmToStep;
	
	moveTo(current_pos.x + x, current_pos.y + y, current_pos.z + z);
}

