Funcional ROV routine and tested with the GUI and the joystick