Modified standalone code to work with go-to-goal instead of go-to-angle.