Fixed time, added printout of distance since start
Prepared obstacle avoidance part