Path planning working

Very basic. Hard coded source.