Package components.robot.mazeresolver

Source Code of components.robot.mazeresolver.MazeResolverRobot

package components.robot.mazeresolver;

import java.awt.Color;
import java.util.Random;

import utils.defines.Defines;
import utils.images.RecognizedShape;
import utils.logging.Logger;

import components.commchannel.CommunicationChannel;
import components.commchannel.CommunicationChannelException;
import components.features.Feature;
import components.features.cam.Camera;
import components.features.engine.Engine;
import components.features.sensor.Sensor;
import components.gps.imagebased.ImageBasedGPS;
import components.imagecollector.virtual.VirtualImageCollector;
import components.robot.Robot;
import components.robot.RobotException;
import components.robot.RobotState;

import core.simulation.Simulation;


/**
* <code>MazeResolverRobot</code> representa la clase base para cualquier
* robot (real o virtual) capaz de resolver un laberinto. Esta clase implementa
* toda la l�gica de navegaci�n del robot en el laberinto, para lo cual toma
* datos de los <i>features</i> (por ejemplo, sensores) que lo conforman.
* No interesa si estos sensores son simulados por software (en el caso de un
* robot virtual), o si son componentes electr�nicos (en un robot real), como
* tampoco interesa la forma en que se le dan �rdenes al robot.
* Todos esos son detalles que resuelve el canal de comunicaciones asociado
* (solo es necesario conocer el protocolo para enviar y recibir datos a
* trav�s de �l).
*/
public class MazeResolverRobot extends Robot
{
 
  // Atributos para control de movimiento
  private java.awt.Color    viewedColor, lastViewColor;
  private Random        randNums;
  private boolean        move, leftRotating, rightRotating;
  private int          leftRotations, rightRotations;

  /* Variables nueva navegacion */
  private static final int UNRECOGNIZED_PLACE = -634;

  enum NAV_STATE{AVANZANDO,
               BUSCANDO_PROXIMO_LUGAR,
               OBTENIENDO_VISTAS_INICIANDO,
               OBTENIENDO_VISTAS_EXPLORANDO,
               CENTRANDO_PROXIMO_LUGAR_IZQUIERDA,
               CENTRANDO_PROXIMO_LUGAR_DERECHA,
               EVITANDO_PARED
               };
              
  private NAV_STATE nav_state;
  private int nav_current_place;
  private int nav_next_place;
  private boolean nav_center_place_enabled;
  private int nav_current_views[];
  private int nav_last_viewed_place;
 
  /**
   * Construye un nuevo robot resolvedor de laberintos, con nombre nulo,
   * posici�n inicial (0.0,0.0), y direcci�n inicial 0.0 rad.
   */
  public MazeResolverRobot()
  {
    super();
    randNums        = new Random();
    move          = true;
    leftRotating      = false;
    rightRotating      = false;
    leftRotations      = 0;
    rightRotations      = 0;
   
    statusTable.put(new Integer(Defines.STATE_ROBOT_NOT_CALIBRATED),"Sin posicionar");
    statusTable.put(new Integer(Defines.STATE_ROBOT_CALIBRATED)"Posicionado!");
  }
 
 
  private final double computeDeltaNewCourse(int currentPlace)
  {
    RobotState state = getState();
   
    double     newCourse = 0.0
    int      nextPlaceId;

    if ( currentPlace != neuralNet.getCurrentPlace() )
    {
      // La red neuronal "piensa" que estamos en un lugar, pero en
      // realidad, el robot est� en otro lugar diferente. Esto puede
      // suceder por las imperfecciones en la navegaci�n del robot.
      // TODO: ��� c�mo manejar esta situaci�n ???
      Logger.warn"Lugar actual en la red neuronal: " +
              neuralNet.getCurrentPlace()     +
              " Lugar actual real del robot: "   +
              currentPlace            );
    }
   
    neuralNet.move( Simulation.getCurrent().getPlaces()[neuralNet.getCurrentPlace()-1] , currentPlace );
    nextPlaceId = neuralNet.getCurrentPlace();
   
    Logger.info( "Actual: " + currentPlace + " Pr�ximo: " + nextPlaceId );
   
    // Busca el lugar pr�ximo en el arreglo de �conos coloreados.
    boolean encontrado = false;
    for (int i=0; !encontrado && i<Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons.length; i++)
    {
      RecognizedShape icon = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons[i];
      if ( icon.shapeId.equals(String.valueOf(nextPlaceId)) )
      {
        // Establece el nuevo curso
        newCourse = Math.atan2( icon.y-state.position[1] , icon.x-state.position[0] ) - state.course;
        encontrado = true;
      }
    }
   
    if ( !encontrado )
    {
      // TODO: qu� hacer si no se encuentra el �cono coloreado?
      // Por ahora, se mantiene el mismo curso.
      Logger.error( "No se encontr� el �cono coloreado con id=" + nextPlaceId );
    }
   
    return newCourse;
  }

  /**
   * Implementa la l�gica de navegaci�n del robot en el laberinto,
   * abstray�ndose de los <i>features</i> (sensores).
   * Este m�todo no debe sobreescribirse en las clases hijas.
   */
  final int currentlyViewedPlace() {
    java.awt.Color viewedColor = (java.awt.Color)((Sensor)commChannel.receive().get(CommunicationChannel.COLOR_SENSOR_NAME)).getValue();
    if(viewedColor == null ) return UNRECOGNIZED_PLACE;
   
    RecognizedShape[] icons = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons;
    for (int i=0; i<icons.length; i++)
      if ( icons[i].color.equals(viewedColor) )

        return Integer.valueOf(icons[i].shapeId).intValue();
   
    return UNRECOGNIZED_PLACE;
  }
 
  public final void move() throws RobotException
  {
   
    if(firstMovement) {
      nav_current_place=UNRECOGNIZED_PLACE;
      nav_next_place=1;
      nav_last_viewed_place=1;
      nav_center_place_enabled=false;
      nav_state=NAV_STATE.AVANZANDO;
      firstMovement=false;
      nav_current_views=new int[Simulation.getCurrent().getPlaces()[0].length];
    }

    Boolean viewing_wall = ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue();
    int viewed_place = currentlyViewedPlace();
    if(viewed_place != UNRECOGNIZED_PLACE) {
      // El ultimo lugar que identifique
      nav_last_viewed_place = viewed_place;
    }
   
    switch(nav_state) {
    case AVANZANDO:
      //System.out.println("AVANZANDO");
      if(viewing_wall) {
        if(viewed_place != UNRECOGNIZED_PLACE) {
          // Estaba avanzando y encontro un lugar
          if(nav_next_place != viewed_place) {
            // La red neuronal "piensa" que estamos en un lugar, pero en
            // realidad, el robot est� en otro lugar diferente. Esto puede
            // suceder por las imperfecciones en la navegaci�n del robot.
           
            Logger.warn"Lugar actual en la red neuronal: "
                  + nav_next_place
                  + " Lugar actual real del robot: "
                  + viewed_place);
            nav_next_place = viewed_place;
          }   
          nav_current_place = nav_next_place;
          if ( nav_current_place == Simulation.getCurrent().getTargetPlace() ) {
            // Se lleg� al objetivo
            reset();
          }
          else {
            nav_state = NAV_STATE.OBTENIENDO_VISTAS_INICIANDO;
          }
        }
        else {
          // Choco contra una pared
          nav_state = NAV_STATE.EVITANDO_PARED;
        }
      }
      else {
        if(viewed_place != nav_next_place) {
          // Perdio de vista el proximo lugar
          nav_state = NAV_STATE.BUSCANDO_PROXIMO_LUGAR;
        }
        else {
          advance();
        }
      }
      break;

    case EVITANDO_PARED:
      if(viewing_wall) {
        turnLeft( Defines.DELTA_PHI_ROTATIONS );       
      }
      else {
        nav_state = NAV_STATE.BUSCANDO_PROXIMO_LUGAR;
      }
      break;
     
    case BUSCANDO_PROXIMO_LUGAR:   
      //System.out.println("BUSCANDO_PROXIMO_LUGAR");
      // TODO: �Que pasa si desde donde esta no puede ver el proximo lugar?
      //       �Puede pasar esta situacion?
     
      if(viewed_place == nav_next_place) { 
        if(nav_center_place_enabled) {
          // Hacemos que el robot se mueva hacia el centro de lo que esta viendo
          // primero rotamos a la izquierda hasta no ver ese color (CENTRANDO A IZQUIERDA)
          // y luego rotamos a la derecha la mitad de veces que rotamos a izquierda (CENTRANDO A DERECHA)
          leftRotations=0;
          rightRotations=0
          nav_state = NAV_STATE.CENTRANDO_PROXIMO_LUGAR_IZQUIERDA;
        }
        else {
          nav_state = NAV_STATE.AVANZANDO;
        }
      }
      else {
        if(leftRotations*Defines.DELTA_PHI_ROTATIONS > 2*Math.PI) {
          // No veo mi lugar objetivo, me perdi
          // Probablemente ni siquiera se donde estoy
          nav_next_place = nav_last_viewed_place;
          leftRotations=0;
        }
        else {
          turnLeft( Defines.DELTA_PHI_ROTATIONS );
          leftRotations++;
        }
      }
      break;
   
    case OBTENIENDO_VISTAS_INICIANDO:
      if(viewed_place == nav_current_place) {
        turnRight( Defines.DELTA_PHI_ROTATIONS );
      }
      else {
        // Antes de explorar vistas, limpio el arreglo
        for(int i=0;i<nav_current_views.length;i++) {
          nav_current_views[i] = 0;
        }
        nav_state = NAV_STATE.OBTENIENDO_VISTAS_EXPLORANDO;
      }
      break;
    case OBTENIENDO_VISTAS_EXPLORANDO:
      if(viewed_place == nav_current_place) {
        // Gire 360� o algo parecido
        // Invoco a la red neuronal para determinar el proximo lugar
        System.out.println("[ROBOT] --- INVOCO RED NEURONAL ---");   
        neuralNet.move( nav_current_views , nav_current_place );
        nav_next_place = neuralNet.getCurrentPlace();
        nav_state = NAV_STATE.BUSCANDO_PROXIMO_LUGAR;       
      }
      else {
        if(viewed_place != UNRECOGNIZED_PLACE) {
          // Marco el color como que lo estoy viendo desde el lugar actual
          // Esta operacion es idempotente, o sea, no importa si lo marco N veces o 1 sola vez.
          if(nav_current_views[viewed_place-1]==0) {
               System.out.println("[ROBOT] Veo el lugar:"+viewed_place);
          }
           nav_current_views[viewed_place-1] = 1;         
        }
        turnRight( Defines.DELTA_PHI_ROTATIONS );       
      }
      break;
    case CENTRANDO_PROXIMO_LUGAR_IZQUIERDA:
      //System.out.println("CENTRANDO A IZQUIERDA");
      if(viewed_place == nav_next_place) {
        turnLeft( Defines.DELTA_PHI_ROTATIONS )
        leftRotations++;
      }
      else {
        //System.out.println("CENTRANDO A IZQUIERDA => A DERECHA");
        nav_state = NAV_STATE.CENTRANDO_PROXIMO_LUGAR_DERECHA;
      }
      break;
    case CENTRANDO_PROXIMO_LUGAR_DERECHA:
      //System.out.println("CENTRANDO A DERECHA");
      if(rightRotations <= leftRotations/2) {
        turnRight( Defines.DELTA_PHI_ROTATIONS )
        rightRotations++;       
      }
      else if(viewed_place == nav_next_place) {
        //System.out.println("CENTRANDO A DERECHA => AVANZAR");
        nav_state = NAV_STATE.AVANZANDO;
      }
      else {
        // No deberia pasar
        turnRight( Defines.DELTA_PHI_ROTATIONS );         
      }
      break;
     
    } // fin switch
  }
 
 
  /**
   * move original de EasyBot
   */
  @Deprecated
  private final void _move_original() throws RobotException
  {
   
    // Avanza
    if ( move )
    {
      try
      {
        advance();
        updateState();
      }
      catch (RobotException e)
      {
        Logger.error( e.getMessage() );
      }
    }

    move = false;
   
    if ( firstMovement )
    {
      turnRight( computeDeltaNewCourse( 1 ) );
      firstMovement = false;
    }
    else if ( leftRotating )
    {
      // Est� rotando a izquierda
     
      boolean aux = ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue();
     
      if ( !aux )
      {
        // Libr�
        leftRotating = false;
       
        if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
        {
          // Ahora empieza a girar a derecha
          rightRotating  = true;
         
          turnRight( Defines.DELTA_PHI_ROTATIONS );
          rightRotations++;
        }
        else
        {
          // Simulaci�n real. Entonces empieza a avanzar.
          leftRotations = 0;
          move = true;
        }
      }
      else
      {
        turnLeft( Defines.DELTA_PHI_ROTATIONS );
        leftRotations++;
      }
    }
    else if ( rightRotating )
    {
      // Est� girando a derecha
     
      if ( ! ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue() )
      {
        // Libr�
        leftRotating  = false;
        rightRotating  = false;

        rightRotations -= leftRotations;

        if ( leftRotations < rightRotations )
        {
          // Gira a izquierda
          for (int i=0; i<leftRotations+rightRotations/*+1*/; i++)
            turnLeft( Defines.DELTA_PHI_ROTATIONS );
        }
     
        leftRotations  = 0;
        rightRotations  = 0;
      }
      else
      {
        turnRight( Defines.DELTA_PHI_ROTATIONS );
        rightRotations++;
      }
    }
    else if (  (viewedColor=(java.awt.Color)((Sensor)commChannel.receive().get(CommunicationChannel.COLOR_SENSOR_NAME)).getValue()) != null   &&
          ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue()        &&
          ( lastViewColor == null || !viewedColor.equals(lastViewColor))    )
    {
      // Est� percibiendo un color.
     
      int currentPlace = -1;
      RecognizedShape[] icons = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons;
      for (int i=0; currentPlace==-&&  i<icons.length; i++)
        if ( icons[i].color.equals(viewedColor) )
          currentPlace = Integer.valueOf(icons[i].shapeId).intValue();

      int[][] places = Simulation.getCurrent().getPlaces();
      if ( currentPlace != -&&  places != null )
      {
        // Calcula el nuevo curso
        turnRight( computeDeltaNewCourse( currentPlace ) );
       
        if ( currentPlace == Simulation.getCurrent().getTargetPlace() )
          // Se lleg� al objetivo
          reset();
      }
      else
      {
        // TODO: ver c�mo manejar la situaci�n en que no
        // se puede resolver el lugar
       
        // Por ahora, asume que si no puede resolver el lugar,
        // contin�a avanzando hacia una posici�n aleatoria
        turnRight( randNums.nextDouble() * 2.0*Math.PI );
      }
     
    }
    else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue() )
    {
      // Est� percibiendo una pared en el frente.
      // Comienza el proceso de rotaci�n.
     
      leftRotating  = true;
      rightRotating  = false;

      assert leftRotations == 0 : "leftRotations = "+leftRotations;
     
      turnLeft( Defines.DELTA_PHI_ROTATIONS );
      leftRotations++;
    }
    else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.LEFT_SENSOR_NAME)).getValue()).booleanValue() )
    {
      // Est� percibiendo una pared en la izquierda
     
      turnRight( randNums.nextDouble() * 0.5*Defines.DELTA_PHI_ROTATIONS );
     
      /*
      // SOLO AVANZAR EN CONDICIONES NORMALES     
      if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
        move = true;
      */
    }
    else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.RIGHT_SENSOR_NAME)).getValue()).booleanValue() )
    {
      // Est� percibiendo una pared en la derecha
     
      turnLeft( randNums.nextDouble() * 0.5*Defines.DELTA_PHI_ROTATIONS );
     
      /*
      // SOLO AVANZAR EN CONDICIONES NORMALES
      if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
        move = true;
      */
    }
    else
    {
      // En condiciones normales, avanza.
      move = true;
    }

  }
 
 
  public void turnLeft(double angle) throws RobotException
  {
    commChannel.send( "left=" + angle );
    update_logical_course(-angle,false);
    updateState();
  }


  public void turnRight(double angle) throws RobotException
 
    commChannel.send( "right=" + angle );
    update_logical_course(+angle,false);
    updateState();
  }


  public void advance() throws RobotException
  {   
    commChannel.send( "advance" );
    update_logical_course(0,true)
    updateState();
  }
 
 
  /**
   * Reinicializa el robot a su estado original, y se invoca al m�todo
   * {@link components.neuralnetwork.NeuralNetwork#reset()
   * reset()} de la red neuronal.
   * @throws RobotException
   */
  public void reset() throws RobotException
  {
    // Se reinicializa la red neuronal.
    if ( neuralNet != null )
      neuralNet.reset();
   
    if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
    {
      // La simulaci�n es virtual (por lo tanto, el robot
      // tambi�n lo es). Entonces, se lo vuelve a la posici�n
      // original.
      ((VirtualImageCollector)((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector())
        .drawRobot(
              new Color(color.getRed(), color.getGreen(), color.getBlue()) ,
              new double[] {initPosition[0], initPosition[1]} );
     
      firstUpdate = true;
      updateState();
     
      Logger.info( "El robot virtual lleg� al objetivo." );
    }
    else
    {
      // TODO: qu� hacer si se lleg� al objetivo, y es
      // un robot real ???
      Logger.info( "El robot real lleg� al objetivo." );
    }
   
    firstMovement = true;
  }

 
  // TODO: ver este m�todo!!! (deber�a estar implementado de una manera
  // m�s gen�rica en la clase ancestra Robot).
  public void addFeature(Feature feature) throws RobotException
  {
    if ( features == null )
      features = new Feature[0];
   
    // Append
    Feature[] temp = features;
    features = new Feature[ temp.length + 1 ];
    for (int i=0; i<temp.length; i++)
      features[i] = temp[i];
    features[ features.length - 1 ] = feature;
   
    try
    {
      commChannel.registerFeature( feature );
    }
    catch (CommunicationChannelException e)
    {
      Logger.error( e.getMessage() );
      throw new RobotException( e.getMessage() );
    }
  }

 
  public String getXMLConfig(String ident)
  {
    String xmlConfig = new String();
    String tabulator = new String();
   
    tabulator = ident;
    xmlConfig = xmlConfig.concat(tabulator + "<robot name=\""+ name + "\"  description=\""+ description +"\">" + Defines.CR);
    tabulator = ident + "\t";
    xmlConfig = xmlConfig.concat(tabulator+ "<color>" + Defines.CR);
    tabulator = ident + "\t\t";
    xmlConfig = xmlConfig.concat(tabulator+ "<r>"+color.getRed()+"</r>" + Defines.CR);
    xmlConfig = xmlConfig.concat(tabulator+ "<g>"+color.getGreen()+"</g>" + Defines.CR);
    xmlConfig = xmlConfig.concat(tabulator+ "<b>"+color.getBlue()+"</b>" + Defines.CR);
    tabulator = ident + "\t";
    xmlConfig = xmlConfig.concat(tabulator+ "</color>" + Defines.CR);   
    xmlConfig = xmlConfig.concat(tabulator+ "<position>"+Defines.CR);
    tabulator= ident + "\t\t";
    xmlConfig = xmlConfig.concat(tabulator + "<x>"+initPosition[0]+"</x>" + Defines.CR);
    xmlConfig = xmlConfig.concat(tabulator + "<y>"+initPosition[1]+"</y>" + Defines.CR);
    tabulator= ident + "\t";
    xmlConfig = xmlConfig.concat(tabulator+ "</position>" + Defines.CR);
    xmlConfig = xmlConfig.concat(commChannel.getXMLConfig(tabulator)+Defines.CR);
    xmlConfig = xmlConfig.concat(neuralNet.getXMLConfig(tabulator)+Defines.CR);
    xmlConfig = xmlConfig.concat(tabulator+ "<features>"+Defines.CR);
    tabulator = ident + "\t\t";
    //Bloque de Sensores
    xmlConfig = xmlConfig.concat(tabulator+ "<sensors>"+Defines.CR);
    tabulator = ident + "\t\t\t";
    for (int i=0 ; i<features.length;i++)
    {
      if (features[i] instanceof Sensor)
      xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator));
    }
    tabulator = ident + "\t\t";
    xmlConfig = xmlConfig.concat(tabulator+ "</sensors>"+Defines.CR);
    //Bloques para engines
    xmlConfig = xmlConfig.concat(tabulator+ "<engines>"+Defines.CR);
    tabulator = ident + "\t\t\t";
    for (int i=0 ; i<features.length;i++)
    {
      if (features[i] instanceof Engine)
      xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator)+Defines.CR);
    }
    tabulator = ident + "\t\t";
    xmlConfig = xmlConfig.concat(tabulator+ "</engines>"+Defines.CR);
    //Bloques para camaras
    xmlConfig = xmlConfig.concat(tabulator+ "<cameras>"+Defines.CR);
    tabulator = ident + "\t\t\t";
    for (int i=0 ; i<features.length;i++)
    {
      if (features[i] instanceof Camera)
      xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator)+Defines.CR);
    }
    tabulator = ident + "\t\t";
    xmlConfig = xmlConfig.concat(tabulator + "</cameras>"+Defines.CR);
    //TODO:Bloques para misc
    tabulator = ident + "\t";
    xmlConfig = xmlConfig.concat(tabulator + "</features>" + Defines.CR);
   
    xmlConfig = xmlConfig.concat("</robot>");
   
    return xmlConfig;
  }

}
TOP

Related Classes of components.robot.mazeresolver.MazeResolverRobot

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.