Moving stepper motor to initial postion and diagonal