Implements path planning and gets it building