Improvement on moving function. Added a loop to reach the position, and security checkings