Fiz inicio codigo para ficar em uma distancia constante da parede.
Por enquanto quando sai da tolerancia ele para.