Motors working, before testing new GoToGoal