Mindstorms NXT - LeJOS (Salir de un laberinto)

El objetivo de este ejercicio consiste en programar al robot, para que utilizando el sensor ultrasónico, pueda localizar obstáculos y girar para evitarlos. El robot también se debe programar para que utilizando estas habilidades (localizar obstáculos y girar) pueda salir de un laberinto de ángulos rectos.

Se muestra a continuación un par de fotografías que ilustran el modelo de robot utilizado, para la resolución del problema:

Solución

Como se comentó hace tres entradas, se utilizó para programar al robot, una arquitectura conocida como “subsumption architecture”; para más información acerca de en qué consiste, consultar la entrada del blog mencionada.

En el caso preciso de este ejercicio, se cuenta con una única capa, que representa el “seguir el contorno de una pared/objeto”.

El código para lograr lo anterior, se encuentra divido en dos clases: WallFollower.java, y FollowWall.java,. La primera de ellas actúa básicamente como un activador del comportamiento descrito en el párrafo anterior.

Se muestra a continuación el código de cada una de las dos clases:

WallFollower.java

import lejos.nxt.*;
import lejos.robotics.navigation.*;
import lejos.robotics.subsumption.*;

public class WallFollower { 
 public static void main(String [] args){
  TachoPilot pilot = new TachoPilot(5.6f, 11.25f, Motor.A, Motor.C);
  UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S3);

  Behavior b1 = new FollowWall(pilot, sonic);
  
  Behavior [] behaviorArray = {b1};
  Arbitrator arbitrator = new Arbitrator(behaviorArray);
  
  LCD.drawString("WallFollower", 2, 2);
  Button.waitForPress();
  arbitrator.start();
 }

}

FollowWall.java

import lejos.nxt.*;
import lejos.robotics.subsumption.*;
import lejos.robotics.navigation.*;

public class FollowWall implements Behavior{
 private TachoPilot pilot;
 private UltrasonicSensor sonic;

 private int distance;
 private int lastDistance;
 private int distanceDifference;
 
 private int veryCloseLeft;
 private int slightlyCloseLeft;
 private int notCloseLeft;
 private int slightlyFarRight;
 private int veryFarRight;
 
 private int basePowerLeft;
 private int basePowerRight;
 
 private Motor rightMotor;
 private Motor leftMotor;

 public FollowWall(TachoPilot pilot, UltrasonicSensor sonic){
  this.pilot = pilot;
  this.sonic = sonic;

  distance = 0;
  lastDistance = 0;
  
  veryCloseLeft = 20;
  slightlyCloseLeft = 25;
  notCloseLeft = 30;
  
  slightlyFarRight = 35;
  veryFarRight = 40;
  
  basePowerLeft = 100;
  basePowerRight = 99;
  
  rightMotor = (Motor)pilot.getRight();
  leftMotor = (Motor)pilot.getLeft();
 }

 public boolean takeControl(){
   LCD.clear();
   LCD.drawString("WallFollow", 2, 2);
   return true;
 }

 public void suppress(){
  pilot.stop();
 }

 public void action(){
  pilot.forward();

  while(true){
   lastDistance = distance;
   distance = sonic.getDistance();
   distanceDifference = lastDistance - distance;
   
   if(distance <= notCloseLeft){
    if(distance <= veryCloseLeft){
     rightMotor.setPower(basePowerRight);
     rightMotor.backward();
     leftMotor.setPower(basePowerRight);
     leftMotor.forward();
    }
    else if(distance <= slightlyCloseLeft){
     rightMotor.setPower(0);
     rightMotor.stop();
     leftMotor.setPower(basePowerLeft);
     leftMotor.forward();
    }
    else if(distance <= notCloseLeft){
     leftMotor.setPower(basePowerLeft);
     leftMotor.forward();
     rightMotor.setPower(0);
     rightMotor.flt();
    }  
   }
   else{
    if(distance >= slightlyFarRight){
     if(distanceDifference < 2){
      leftMotor.setPower(0);
      leftMotor.stop();
      rightMotor.setPower(basePowerRight);
      rightMotor.forward();
     }
    }
    else if(distance >= veryFarRight){
     leftMotor.setPower(basePowerLeft/2);
     leftMotor.forward();
     rightMotor.setPower(basePowerLeft);
     rightMotor.forward();
    }
   }
   try {
    Thread.sleep(30);
   } catch (InterruptedException e) {}
  }
 }
}

Finalmente, se adjunta un par de videos, donde se puede apreciar al robot ejecutando la tarea encomendada: