Cleaning and Improvements

Now It's avoiding obstacles