Desarrollo de un seguidor de paredes inteligente (Parte 7 de 7: Consideraciones finales)

Ahora que tenemos preparado nuestro controlador difuso para el seguimiento de paredes, hace falta un código cliente que haga uso de él y comenzar las pruebas.

Se dieron problemas durante las simulaciones y se debía a que los sensores no eran capaces de trabajar a la velocidad a la que iteraba el programa.

Para solucionar el problema basta con variar el tiempo de iteración con Thread.sleep (300)

El código es:

public class Principal{  
 PlayerClient        robot = null;
 Position2DInterface posi  = null;
 SonarInterface      soni  = null;
 ControladorFuzzy controladorOrientacion = null;
 
  
 public Principal(){
  float temp[];
  float xSpeed, yawSpeed;
  
  try {
   // Connect to the Player server and request access to Position and Sonar
   robot  = new PlayerClient ("localhost", 6665);
   posi = robot.requestInterfacePosition2D (0,PlayerConstants.PLAYER_OPEN_MODE);
   soni = robot.requestInterfaceSonar(0,PlayerConstants.PLAYER_OPEN_MODE);
  } catch (PlayerException e) {
   System.err.println ("WallFollowerExample: &mt; Error connecting to Player: ");
   System.err.println ("    [ " + e.toString() + " ]");
   System.exit (1);
  }
  
  controladorOrientacion = new ControladorFuzzy("datosAngular.pwm", "datosAngular.wm");
  
  robot.runThreaded (-1, -1);
   
  
      xSpeed = yawSpeed = 0;
      float DD, DF;  // Distancias por la derecha (actual y anterior)
  float AE = 0; // Incremento del error
  float DD_REFERENCIA = new Float(1 / Math.cos(Math.PI/6)).floatValue();    // Distancia a la que intentamos mantenernos
  float errorActual, errorAnterior = 0f;
  ArrayList a = new ArrayList();
   
  while (true){
      // Hago un barrido de sónar
   temp = getSonar();
   
   DD = temp[5]; //Distancia por la derecha
   DF = temp[3];
   errorActual = DD - DD_REFERENCIA;
   
    
   AE = errorActual - errorAnterior;
   
  
   //CÓDIGO DE NAVEGACIÓN CON CONTROLADOR DIFUSO
  
   //Control de la velocidad (no es necesario controlador)
   if ((DF < 0.5f))
    xSpeed = MIN_XSPEED_VALUE;
   else
    xSpeed = MAX_XSPEED_VALUE;
    
   
   //Controlador de giro
   a.add(new Float(AE));
   a.add(new Float(DF));
   a.add(new Float(errorActual));
   
   //Controlador de dirección
   
   yawSpeed = controladorOrientacion.evalua(a);
   
   System.out.println(yawSpeed);
   
   a.clear();
  
   //FIN DEL CÓDIGO DE NAVEGACIÓN CON CONTROLADOR DIFUSO
  
   posi.setSpeed(xSpeed, yawSpeed);
      
   errorAnterior = errorActual;
      
  try { Thread.sleep (300); } catch (Exception e) { System.out.println(e);}
 }
  
}
  
 public float[] getSonar(){
  float distancias[];
  
  while (!soni.isDataReady());
  distancias = soni.getData().getRanges();
  
  // Filtrado de valores fuera del rango [SONAR_MIN_VALUE; SONAR_MAX_VALUE]
  for (int i = 0; i < soni.getData ().getRanges_count (); i++)
 
   if (distancias[i] < SONAR_MIN_VALUE)
    distancias[i] = SONAR_MIN_VALUE;
   else
    if (distancias[i] &mt; SONAR_MAX_VALUE)
     distancias[i] = SONAR_MAX_VALUE;
  
  return distancias;
 }
  
  
 public float acota(float valor, float min, float max){
  if (valor < min)
   valor = min;
  else if (valor &mt; max)
   valor = max;
  return valor;
 }
  
  
}
 
 
 
public static void main (String[] args) {
 SeguidorParedes sp = new SeguidorParedes();
 Principal p = sp.new Principal();
  
}