Obstacle Avoidance has been added. Furthermore I have made some minor changes.