Added existing code

Implements navigation alrorithm without obstacles.