Minor changes to controller
Finding nearest road is now precise, and KDTree should have a nicer distribution