Fixed hard coded, improved path planning