Almost finished main autonomous method