}
public void run()
{
Sensor sensor;
RobotState robotState;
double[] pos;
double course;
boolean finished1, finished2, finished3;
int i;
Logger.info( "El canal de comunicaciones comienza su ejecuci�n." );
int width = ((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector().getImageResolution().width;
int height = ((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector().getImageResolution().height;
if ( width <= 0 ) width = Defines.DEFAULT_IMAGE_RESOLUTION[0];
if ( height <= 0 ) height = Defines.DEFAULT_IMAGE_RESOLUTION[1];
while ( ! isClosed() )
{
// Duerme durante un intervalo
try {
Thread.sleep( SLEEP_TIME );
} catch (InterruptedException e) { }
// TODO: cuidado con �sto!!! En este punto, lo �nico que me
// ayuda para que el sem�foro no sea null es el sleep() anterior
// (de otra forma, pinchar�a). Implementarlo bien!
getCommChannelSemaphore().WAIT();
if ( Simulation.getCurrent().getGps() != null )
sceneItems = Simulation.getCurrent().getGps().getMazeItems();
else
sceneItems = null;
if ( sceneItems == null )
{
Logger.error("No hay �tems reconocidos en el escenario.");
continue;
}
robotState = robot.getState();
pos = robotState.position;
course = realCourse;
// Calcula las coordenadas de los puntos laterales, y del punto frontal.
state.frontX = pos[0] + Defines.ROBOT_RADIO * Math.cos( course );
state.frontY = pos[1] + Defines.ROBOT_RADIO * Math.sin( course );
state.leftX = pos[0] + Defines.ROBOT_RADIO * Math.cos( course + 3.0*Math.PI/2.0);
state.leftY = pos[1] + Defines.ROBOT_RADIO * Math.sin( course + 3.0*Math.PI/2.0);
state.rightX = pos[0] + Defines.ROBOT_RADIO * Math.cos( course + Math.PI/2.0 );
state.rightY = pos[1] + Defines.ROBOT_RADIO * Math.sin( course + Math.PI/2.0 );
// Calcula los puntos de alcance de todos los sensores.
state.frontSensorX = state.frontX + Defines.FRONT_SENSOR_DISTANCE * Math.cos(course);
state.frontSensorY = state.frontY + Defines.FRONT_SENSOR_DISTANCE * Math.sin(course);
state.leftSensorX = state.leftX + Defines.LEFT_SENSOR_DISTANCE * Math.cos(Math.PI/2.0 - course);
state.leftSensorY = state.leftY - Defines.LEFT_SENSOR_DISTANCE * Math.sin(Math.PI/2.0 - course);
state.rightSensorX = state.rightX - Defines.RIGHT_SENSOR_DISTANCE * Math.cos(Math.PI/2.0 - course);
state.rightSensorY = state.rightY + Defines.RIGHT_SENSOR_DISTANCE * Math.sin(Math.PI/2.0 - course);
//////////////////////////////////////////
// //
// Se analiza el SENSOR FRONTAL //
// //
//////////////////////////////////////////
finished1 = false;
for ( i=0; i<sceneItems.recognizedColoredIcons.length && !finished1; i++)
{
if ( detect( FRONT_SENSOR , i, GPS.GPS_COLORS) )
// El sensor frontal detecta un �cono coloreado
finished1 = true;
}
finished2 = false;
for ( i=0; i<sceneItems.recognizedWalls.length && !finished2; i++)
{
if ( detect( FRONT_SENSOR , i, GPS.GPS_WALLS) )
// El sensor frontal detecta una pared
finished2 = true;
}
finished3 = false;
/*
for ( i=0; i<sceneItems.recognizedRobots.length && !finished3; i++)
{
if ( detect( FRONT_SENSOR , i, GPS.GPS_ROBOTS) )
// El sensor frontal detecta otro robot
finished3 = true;
}
*/
if ( finished1 || finished2 || finished3 )
{
// El sensor frontal est� detectando algo
sensor = (ProximitySensor)features.featuresHash.get( FRONT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(true) );
else
// TODO: ver c�mo manejar la situaci�n en que no se encuentra
// el feature en el hash.
Logger.error("No se pudo acceder al sensor frontal.");
}
else
{
// No se detecta ninguna pared al frente
sensor = (ProximitySensor)features.featuresHash.get( FRONT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(false) );
}
//////////////////////////////////////////
// //
// Se analiza el SENSOR IZQUIERDO //
// //
//////////////////////////////////////////
finished1 = false;
for ( i=0; i<sceneItems.recognizedColoredIcons.length && !finished1; i++)
{
if ( detect( LEFT_SENSOR , i, GPS.GPS_COLORS) )
// El sensor izquierdo detecta un �cono coloreado
finished1 = true;
}
finished2 = false;
for ( i=0; i<sceneItems.recognizedWalls.length && !finished2; i++)
{
if ( detect( LEFT_SENSOR , i, GPS.GPS_WALLS) )
// El sensor izquierdo detecta una pared
finished2 = true;
}
finished3 = false;
/*
for ( i=0; i<sceneItems.recognizedRobots.length && !finished3; i++)
{
if ( detect( LEFT_SENSOR , i, GPS.GPS_ROBOTS) )
// El sensor izquierdo detecta otro robot
finished3 = true;
}
*/
if ( finished1 || finished2 || finished3 )
{
// El sensor izquierdo est� detectando algo
sensor = (ProximitySensor)features.featuresHash.get( LEFT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(true) );
else
// TODO: ver c�mo manejar la situaci�n en que no se encuentra
// el feature en el hash.
Logger.error("No se pudo acceder al sensor izquierdo.");
}
else
{
// No se detecta ninguna pared a la izquierda
sensor = (ProximitySensor)features.featuresHash.get( LEFT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(false) );
else
// TODO: ver c�mo manejar la situaci�n en que no se encuentra
// el feature en el hash.
Logger.error("No se pudo acceder al sensor izquierdo.");
}
//////////////////////////////////////////
// //
// Se analiza el SENSOR DERECHO //
// //
//////////////////////////////////////////
finished1 = false;
for ( i=0; i<sceneItems.recognizedColoredIcons.length && !finished1; i++)
{
if ( detect( RIGHT_SENSOR , i, GPS.GPS_COLORS) )
// El sensor derecho detecta un �cono coloreado
finished1 = true;
}
finished2 = false;
for ( i=0; i<sceneItems.recognizedWalls.length && !finished2; i++)
{
if ( detect( RIGHT_SENSOR , i, GPS.GPS_WALLS) )
// El sensor derecho detecta una pared
finished2 = true;
}
finished3 = false;
/*
for ( i=0; i<sceneItems.recognizedRobots.length && !finished3; i++)
{
if ( detect( RIGHT_SENSOR , i, GPS.GPS_ROBOTS) )
// El sensor derecho detecta otro robot
finished3 = true;
}
*/
if ( finished1 || finished2 || finished3 )
{
// El sensor derecho est� detectando algo
sensor = (ProximitySensor)features.featuresHash.get( RIGHT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(true) );
// TODO: ver c�mo manejar la situaci�n en que no se encuentra
// el feature en el hash.
}
else
{
// No se detecta ninguna pared a la derecha
sensor = (ProximitySensor)features.featuresHash.get( RIGHT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(false) );
}
//////////////////////////////////////////
// //
// Se analiza el SENSOR DE COLOR //
// //
//////////////////////////////////////////
double xl = pos[0] + 0.5*Defines.ROBOT_RADIO * Math.cos( course + 3.0*Math.PI/2.0);
double yl = pos[1] + 0.5*Defines.ROBOT_RADIO * Math.sin( course + 3.0*Math.PI/2.0);
double xr = pos[0] + 0.5*Defines.ROBOT_RADIO * Math.cos( course + Math.PI/2.0 );
double yr = pos[1] + 0.5*Defines.ROBOT_RADIO * Math.sin( course + Math.PI/2.0 );
Color colorLeft = null;
Color colorRight= null;
// Se analiza el "flanco" izquierdo
finished1 = false;
finished2 = false;
finished3 = false;
while ( !finished1 && !finished2 && !finished3 )
{
xl += Defines.STEP_SIZE * Math.cos( course );
yl += Defines.STEP_SIZE * Math.sin( course );
for (i=0; !finished1 && i<sceneItems.recognizedWalls.length; i++)
if ( sceneItems.recognizedWalls[i].contains(xl, yl) )
// Hay una pared
finished1 = true;
// TODO: verificar que no est� chocando contra otro robot
for (i=0; !finished1 && !finished2 && !finished3 && i<sceneItems.recognizedColoredIcons.length; i++)
if ( sceneItems.recognizedColoredIcons[i].contains(xl, yl) )
{
// Encontr� un color
sensor = (ColorSensor)features.featuresHash.get( COLOR_SENSOR_NAME );
if ( sensor != null )
colorLeft = sceneItems.recognizedColoredIcons[i].color;
finished3 = true;
}
if ( xl < 0 || xl > width || yl < 0 || yl > height )
// Se fue del rango de la imagen...
break;
}
// Se analiza el "flanco" derecho
finished1 = false;
finished2 = false;
finished3 = false;
while ( !finished1 && !finished2 && !finished3 )
{
xr += Defines.STEP_SIZE * Math.cos( course );
yr += Defines.STEP_SIZE * Math.sin( course );
for (i=0; !finished1 && i<sceneItems.recognizedWalls.length; i++)
if ( sceneItems.recognizedWalls[i].contains(xr, yr) )
// Hay una pared
finished1 = true;
// TODO: verificar que no est� chocando contra otro robot
for (i=0; !finished1 && !finished2 && !finished3 && i<sceneItems.recognizedColoredIcons.length; i++)
if ( sceneItems.recognizedColoredIcons[i].contains(xr, yr) )
{
// Encontr� un color
sensor = (ColorSensor)features.featuresHash.get( COLOR_SENSOR_NAME );
if ( sensor != null )
colorRight = sceneItems.recognizedColoredIcons[i].color;
finished3 = true;
}
if ( xr < 0 || xr > width || yr < 0 || yr > height )
// Se fue del rango de la imagen...
break;
}
sensor = (ColorSensor)features.featuresHash.get( COLOR_SENSOR_NAME );
if ( colorLeft != null && colorRight != null && colorLeft.equals(colorRight) )
{
// Ambos "flancos" detectan el mismo color. Entonces, se considera
// que el robot est� viendo ese color.
if ( sensor != null )
sensor.setValue( colorLeft );
}
else
{
// No encontr� color
if ( sensor != null )
sensor.setValue( null );
}
getCommChannelSemaphore().SIGNAL();