PID controller based trajectory finder
Move vector to a seperate file
Getting ready to implement GA