Mindstorms NXT - LeJOS (Rodear un objeto)

El objetivo de este ejercicio consiste en programar al robot, para que utilizando el sensor de tacto, pueda encontrar, y rodear un objeto siguiendo su contorno. El objeto puede tener cualquier forma (rectangular, como una caja o cilíndrica como un bote), asimismo, el objeto será lo suficientemente pesado como para que el robot no lo mueva, dentro de límites razonables.

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 dos 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 la capa más baja representa el “moverse hacia delante”; por su parte, la capa más elevada representa el comportamiento de “reconocer que se ha chocado, retirarse del objeto que se tiene enfrente una distancia determinada, rotar de tal manera que el sensor ultrasónico quede mirando en dirección al objeto, y rodearlo”; ésta capa toma control cuando se ha detectado una colisión por medio del sensor de tacto.

El código para lograr lo anterior, se encuentra divido en tres clases: BWallFollower.java, DriveForward.java, y HitWall.java,. La primera de ellas actúa básicamente como un regulador, para decidir qué comportamientos deben ser activados, mientras que las otras dos, aluden a los comportamientos descritos en el párrafo anterior, de manera correspondiente.

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

BWallFollower.java

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

public class BWallFollower { 
 public static void main(String [] args){
  TachoPilot pilot = new TachoPilot(5.6f, 11.25f, Motor.A, Motor.C);
  TouchSensor touch = new TouchSensor(SensorPort.S1);
  UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S3);
  
  Behavior b1 = new DriveForward(pilot);
  Behavior b2 = new HitWall(pilot, touch, sonic);
  
  Behavior [] behaviorArray = {b1, b2};
  Arbitrator arbitrator = new Arbitrator(behaviorArray);
  
  LCD.drawString("BWallFollower", 2, 2);
  Button.waitForPress();
  arbitrator.start();
 }

}

DriveForward.java

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

public class DriveForward implements Behavior{
 
 private Pilot pilot;
 
 public DriveForward(Pilot pilot){
  this.pilot = pilot;
 }
  
 public boolean takeControl(){
  LCD.clear();
  LCD.drawString("DriveForward", 2, 2);
  return true;
 }
 
 public void suppress(){
  pilot.stop();
 }
 
 public void action(){
  pilot.forward();
 }

}

HitWall.java

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


public class HitWall implements Behavior, SensorPortListener{
 private TachoPilot pilot;
 private TouchSensor touch;
 private UltrasonicSensor sonic;
 private boolean hasCollided;

 private int backoff;
 private int rotationAngle;
 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 HitWall(TachoPilot pilot, TouchSensor touch, UltrasonicSensor sonic){
  this.pilot = pilot;
  this.touch = touch;
  this.sonic = sonic;

  hasCollided = false;
  SensorPort.S1.addSensorPortListener(this);

  backoff = 5;
  rotationAngle = -90;
  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 void stateChanged(SensorPort port, int oldValue, int newValue){
  if(touch.isPressed())
   hasCollided = true;
 }

 public boolean takeControl(){
  if(hasCollided){
   LCD.clear();
   LCD.drawString("HitObject", 2, 2);
   hasCollided = false;
   return true;
  } else
   return false;
 }

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

 public void action(){
  pilot.stop();
  pilot.travel(-backoff);
  pilot.rotate(rotationAngle);
  
  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: