Mindstorms NXT - LeJOS (Ubicar y encontrar una fuente de sonido)

El objetivo de este ejercicio consiste en programar el robot, para que utilizando el sensor de sonido (micrófono), pueda ubicar e ir en dirección de una fuente de sonido.

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ó en la entrada anterior, 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 anterior.

En el caso preciso de este ejercicio la capa más baja representa el “no escuchar a la fuente de sonido”; el robot al encontrarse en esta situación, comienza a hacer giros incrementales, de izquierda a derecha, hasta que vuelve a encontrar la fuente; es en este momento, que toma control la capa más elevada, y que básicamente se encarga de mover al robot hacia delante, mientras lo que el sensor de sonido “escuche”, sea un determinado nivel de decibeles.

El código para lograr lo anterior, se encuentra divido en tres clases: SoundFollower.java, OutOfSound.java, y DriveForward.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:


SoundFollower.java

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

public class SoundFollower {
 
 public static void main(String[] args){
  Pilot pilot = new TachoPilot(5.6f, 11.5f, Motor.A, Motor.C);
  SoundSensor sound = new SoundSensor(SensorPort.S3);
  pilot.setTurnSpeed(180);
  
  Behavior b1 = new OutOfSound(pilot, sound);
  Behavior b2 = new DriveForward(pilot, sound);
  Behavior [] behaviorArray = {b1, b2};
  Arbitrator arbitrator = new Arbitrator(behaviorArray);
  
  LCD.drawString("SoundFollower", 2, 2);
  Button.waitForPress();
  arbitrator.start();
 }

}

OutOfSound.java

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

public class OutOfSound implements Behavior, SensorPortListener{

 private Pilot pilot;
 private SoundSensor sound;
 private int sweep;
 private boolean suppress;
 private boolean outOfSound;

 public OutOfSound(Pilot pilot, SoundSensor sound){
  this.pilot = pilot;
  this.sound = sound;
  suppress = false;
  outOfSound = false;
  SensorPort.S3.addSensorPortListener(this);
 }

 public void stateChanged(SensorPort port, int oldValue, int newValue){
  if(sound.readValue() < 40)
   outOfSound = true;
 }

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

 public void suppress(){
  suppress = true;
 }

 public void action(){
  sweep = 10;
  while(!suppress){
   pilot.rotate(sweep, true);
   while(!suppress && pilot.isMoving())
    Thread.yield();
   sweep*=-2;
  }
  pilot.stop();
  suppress = false;
 }
}

DriveForward.java

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

public class DriveForward implements Behavior, SensorPortListener{
 
 private Pilot pilot;
 private SoundSensor sound;
 private boolean heardSound;
 
 public DriveForward(Pilot pilot, SoundSensor sound){
  this.pilot = pilot;
  this.sound = sound;
  heardSound = false;
  SensorPort.S3.addSensorPortListener(this);
 }
 
 public void stateChanged(SensorPort port, int oldValue, int newValue){
  if(sound.readValue() >= 40)
   heardSound = true;  
 }
 
 public boolean takeControl(){
  if(heardSound){
   LCD.clear();
   LCD.drawString("DriveForward", 2, 2);
   heardSound = false;
   return true;
  } else
   return false;
 }
 
 public void suppress(){
  pilot.stop();
 }
 
 public void action(){
  pilot.forward();
  while(sound.readValue() >= 40)
   Thread.yield();
 }

}

Finalmente, se adjunta un video, donde se puede apreciar al robot ejecutando la tarea encomendada: