Changed some parameters on control_motion. Control_logic is too simple to work properly.