Práctica 2 { parte 5: Comportamiento sigue-pared para salir de un laberinto}

El algoritmo más simple para salir de un laberinto es seguir una misma pared sin soltarla hasta la salida. Por eso, en esta parte programaremos el robot para que siga una pared a una cierta distancia.

Para ello conectaremos al robot el sensor de ultrasonidos al puerto 1, y orientado 45º a la izquierda para que enfoque a la pared.

Aqui podemos ver una foto del montaje:

El codígo consistirá en mientras no presionemos el botón ESCAPE, que haga dos bucles separados. El primero comprobará si está más cerca de la pared de la distancia que le ponemos como límite. De ser así se parará, rotará hacia el lado contrario de la pared y volverá a medir para ver si puede salir del bucle. El otro bucle será si se aleja de la pared, acercarlo, rotando al lado contrario.
Entre ambos bucles se desplazará hacia delante.

Para desplazarlo, pararlo y girarlo usaremos lo métodos de la clase TachoPilot, stop,travel y rotate.

En resumen, hasta que queramos salir, irá moviendose mientras va tomando medidas, si mide que está cerca de la pared se aleja, y si ve que está lejos se acerca.

Como dato curioso, comenar que para este objetivo es muy importante la orientación del sensor, porque nos ha ocurrido que si estaba en una posición incorrecta (60º o 30º por ejemplo) no seguía correctamente las esquinas ya que tomaba otra medida diferente y giraba sobre sí mismo.

Aquí vemos el vídeo del funcionamiento correcto siguiendo varias esquinas, una puerta, el marco de una puerta...:




Aquí podemos ver el código:

import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;


public class Laberinto {

public static void main(String[] args) throws InterruptedException {

Motor Rizda = Motor.C;

Motor Rdcha = Motor.B;

UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);

TachoPilot viajar = new TachoPilot(5.6f,10.85f,Rizda,Rdcha);

sonar.continuous();

viajar.forward();

while (!Button.ESCAPE.isPressed()){

int dist = sonar.getDistance();

while (dist< 25){

viajar.stop();

viajar.rotate(-10);

dist = sonar.getDistance();

}

viajar.travel(5);

while (dist>35){

viajar.stop();

viajar.rotate(10);

dist = sonar.getDistance();

}

viajar.stop();

viajar.travel(5);

}

}

}