Obstacle dodging algorithm almost finished, more tests pending