Actaully fixed path following, I forgot something in my math while I was tired