Working on bezier stuff
Change over to Bezier smoothing.
replanWhenNeeded needs to be reverted and fixed.
Need to implement using Vo and Vf
Change to always use the optomized path and the bezier curve
Change back so it replans when the goal changes
change PID to not accept nan values
Fix very stupid mistake setting vi.y = vi.x
Merge remote-tracking branch 'origin/bezierAlbert' into albert/simulatorFixes