Empezando la implementación del algoritmo (No tiene en cuenta los obstaculos)