Path planning is now working