package components.robot.mazeresolver.real;
import java.awt.Color;
import utils.ColorVectorMap;
import utils.defines.Defines;
import utils.images.RecognizedShape;
import components.commchannel.CommunicationChannel;
import components.commchannel.serial.SerialCommunicationChannel;
import components.commchannel.serial.frontcam.RobotCamColorTrack;
import components.commchannel.serial.frontcam.VectorValues;
import components.features.sensor.Sensor;
import components.robot.RobotException;
import components.robot.mazeresolver.MazeResolverRobot;
import core.simulation.Simulation;
/**
* Representa un robot real capaz de resolver un laberinto.
*/
public class RealMazeResolverRobot extends MazeResolverRobot
{
private NAV_STATE nav_state;
private int nav_current_place;
private int nav_next_place;
private int nav_current_views[];
private int nav_last_viewed_place;
enum NAV_STATE{AVANZANDO,
BUSCANDO_PROXIMO_LUGAR,
OBTENIENDO_VISTAS_INICIANDO,
OBTENIENDO_VISTAS_EXPLORANDO,
CENTRANDO_PROXIMO_LUGAR_IZQUIERDA,
CENTRANDO_PROXIMO_LUGAR_DERECHA,
EVITANDO_PARED
};
public final void move() throws RobotException
{
if(firstMovement) {
nav_current_place=UNRECOGNIZED_PLACE;
nav_next_place=1;
nav_last_viewed_place=1;
nav_state=NAV_STATE.AVANZANDO;
firstMovement=false;
nav_current_views=new int[Simulation.getCurrent().getPlaces()[0].length];
}
Simulation.getCurrent().getRealRobotTurn().WAIT();
Boolean collision;
int viewed_place;
int moveHint;
Boolean inPlace;
try {
collision = ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue();
viewed_place = currentlyViewedPlace();
inPlace = ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.IN_PLACE)).getValue()).booleanValue();
moveHint = ((Integer)((Sensor)commChannel.receive().get(CommunicationChannel.MOVE_HINT)).getValue()).intValue();
} catch(Exception e) {
collision = false;
viewed_place = UNRECOGNIZED_PLACE;
inPlace = false;
moveHint = RobotCamColorTrack.MOVER_ADELANTE;
log.error("[ROBOT] Error al leer los sensores",e);
e.printStackTrace();
}
if(viewed_place != UNRECOGNIZED_PLACE) {
nav_last_viewed_place = viewed_place; // El ultimo lugar que identifique
}
log.info("[ROBOT] Estado Actual: "+nav_state);
log.info("[ROBOT] current_place="+nav_current_place+ " - viewed_place="+viewed_place);
if(inPlace && nav_state==NAV_STATE.EVITANDO_PARED) {
nav_state = NAV_STATE.AVANZANDO;
}
switch(nav_state)
{
case AVANZANDO:
if(collision) {
if(inPlace) {
log.debug("[ROBOT] AVANZANDO. COLISION. IN 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.
log.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;
// TODO meter l�gica adicional para entender que llegamos a destino
// OBJETIVO FINAL
if ( nav_current_place == Simulation.getCurrent().getTargetPlace() ) {
log.debug("[ROBOT] El robot llego al objetivo");
reset();
}
else {
nav_state = NAV_STATE.OBTENIENDO_VISTAS_INICIANDO;
}
}
else {
log.debug("[ROBOT] AVANZANDO. COLISION. PROX. ESTADO: EVITANDO PARED");
// Choco contra una pared
nav_state = NAV_STATE.EVITANDO_PARED;
}
}
else {
if(viewed_place != nav_next_place) {
nav_state = NAV_STATE.BUSCANDO_PROXIMO_LUGAR;
}
else {
advance();
}
}
break;
case EVITANDO_PARED:
if(collision) {
if(moveHint == RobotCamColorTrack.MOVER_IZQUIERDA) {
log.debug("[ROBOT] EVITANDO_PARED. COLISION="+collision+" - GIRO IZQ");
turnLeftByPower( 3 );
}
else {
log.debug("[ROBOT] EVITANDO_PARED. COLISION="+collision+" - GIRO DER");
turnRightByPower( 2 );
}
}
else {
log.debug("[ROBOT] EVITANDO_PARED. COLISION="+collision+" - FIN COLISION -> PASA A BUSCANDO_PROXIMO_LUGAR");
nav_state = NAV_STATE.BUSCANDO_PROXIMO_LUGAR;
}
break;
case BUSCANDO_PROXIMO_LUGAR:
//log.info("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) {
leftRotations=0;
nav_state = NAV_STATE.AVANZANDO;
}
else {
// TODO buscar una mejor manera de resolver el problema de que no veo mi prox lugar,
// quiz�s un abanico
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;
log.debug("[ROBOT] Busc. Prox. Lugar TERMINADO. Vueltas: "+leftRotations);
}
else {
turnLeft( Defines.DELTA_PHI_ROTATIONS );
leftRotations++;
log.debug("[ROBOT] Busc. Prox. Lugar EN CURSO. Vueltas: "+leftRotations);
}
}
break;
case OBTENIENDO_VISTAS_INICIANDO:
log.debug("[ROBOT] OBTENIENDO_VISTAS_INICIANDO. Viewed_place = "+viewed_place+" - nav_current_place = "+nav_current_place);
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 != UNRECOGNIZED_PLACE) {
if(viewed_place == nav_current_place) {
// Gire 360� o algo parecido - Invoco a la red neuronal para determinar el proximo lugar
log.info("[ROBOT] --- INVOCO RED NEURONAL ---");
neuralNet.move( nav_current_views , nav_current_place );
nav_next_place = neuralNet.getCurrentPlace();
log.info("[ROBOT] --- RED NEURONAL DEVUELVE LUGAR: "+nav_next_place);
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) {
log.info("[ROBOT] Identifique el lugar:"+viewed_place);
}
nav_current_views[viewed_place-1] = 1;
}
turnRight( Defines.DELTA_PHI_ROTATIONS );
// este sleep hace que tenga tiempo de estabilizar los colores
// try {
// Thread.sleep(2*SerialCommunicationChannel.MOVE_TIME);
// } catch (InterruptedException e) {
// e.printStackTrace();
// }
}
}
break;
} // fin switch
Simulation.getCurrent().getSerialTurn().SIGNAL();
}
public void turnLeft(double angle) throws RobotException
{
int power = getPowerByAngle(angle);
commChannel.send( "left=" + power );
update_logical_course(-angle,false);
updateState();
}
public void turnLeftByPower(int power) throws RobotException
{
commChannel.send( "left=" + power );
//update_logical_course(power,false);
updateState();
}
public void turnRightByPower(int power) throws RobotException
{
commChannel.send( "right=" + power );
//update_logical_course(+angle,false);
updateState();
}
private int getPowerByAngle(double angle) {
return 5;
}
public void turnRight(double angle) throws RobotException
{
int power = getPowerByAngle(angle);
commChannel.send( "right=" + power );
update_logical_course(+angle,false);
updateState();
}
public void advance() throws RobotException
{
commChannel.send( "advance" );
update_logical_course(0,true);
updateState();
}
/**
* Devuelve el color que se est� viendo.
*
* En la simulaci�n real se utiliza el mapeo entre lo que el usuario eligi�
* en la pantalla y los vectores del archivo (ver clase Defines)
*/
protected int currentlyViewedPlace() {
VectorValues viewedPlace = (VectorValues)((Sensor)commChannel.receive().get(CommunicationChannel.COLOR_SENSOR_NAME)).getValue();
RecognizedShape[] icons = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons;
for (int i=0; i<icons.length; i++) {
java.awt.Color c1 = icons[i].color;
org.eclipse.swt.graphics.Color c2 = ColorVectorMap.getInstance().getColor(viewedPlace.getDescription());
if(c1 == null || c2==null) continue;
if ( c1.getRed() == c2.getRed() &&
c1.getGreen() == c2.getGreen() &&
c1.getBlue() == c2.getBlue() ) return Integer.valueOf(icons[i].shapeId).intValue();
}
return UNRECOGNIZED_PLACE;
}
}