package components.commchannel.serial;
import gnu.io.CommPortIdentifier;
import gnu.io.SerialPort;
import java.io.BufferedReader;
import java.io.FileInputStream;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.util.Hashtable;
import java.util.Properties;
import utils.defines.Defines;
import utils.logging.Logger;
import components.commchannel.CommunicationChannel;
import components.commchannel.serial.frontcam.RobotCamColorTrack;
import components.features.sensor.Sensor;
import components.features.sensor.color.ColorSensor;
import components.features.sensor.proximity.ProximitySensor;
import components.gps.RecognizedItems;
import components.gps.imagebased.ImageBasedGPS;
import components.robot.Robot;
import components.robot.RobotState;
import core.simulation.Simulation;
/**
* Esta clase implementa un canal serie (para comunicaci�n contra
* robots reales).
*/
public class SerialCommunicationChannel extends CommunicationChannel
{
// Constantes
private static final int SLEEP_TIME = 100; // ms
// Atributos
private CommPortIdentifier portId;
private SerialPort serialPort;
private RobotCamColorTrack frontCam;
private BufferedReader in;
private OutputStream out;
private Properties props;
private Robot robot;
private String connectionString;
private int speed;
private int dataBits;
private int stopBit;
private int parity;
private byte[] FRONT_SENSOR;
private byte[] RIGHT_SENSOR;
private byte[] LEFT_SENSOR;
private byte[] ADVANCE;
private byte[] BRAKE;
private byte[] RIGHT_ROTATE;
private byte[] LEFT_ROTATE;
private int SENSOR_THRESHOLD;
private int MOVE_TIME;
private String frontCamPort = "";
private String robotComPort = "";
/**
* Construye un nuevo canal serie de comunicaciones con nombre y
* descripci�n nulos, y no asociado a ning�n robot.
*/
public SerialCommunicationChannel()
{
super();
// Valores Default
speed = 38400;
dataBits = SerialPort.DATABITS_8;
stopBit = SerialPort.STOPBITS_1;
parity = SerialPort.PARITY_NONE;
this.frontCamPort = "COM3";
this.robotComPort = "COM4";
}
/**
* Construye un nuevo canal serie de comunicaciones con nombre
* <code>name</code>, descripci�n <code>description</code>.
* @param name nombre.
* @param description descripci�n.
*/
public SerialCommunicationChannel(String name, String description)
{
super(name, description);
}
public int open(String connectionStr)
{
String robotName, robotComPort, frontCamPort;
if ( connectionStr == null )
return Defines.ERROR;
connectionString = new String(connectionStr);
if ( !connectionString.matches("[^,]*,[^,]*") )
return Defines.ERROR;
String[] connectionParams = connectionString.split(",");
robotName = connectionParams[0].trim();
try
{
if(frontCam!=null)
frontCam.close();
else
frontCam = new RobotCamColorTrack(this.frontCamPort);
portId = CommPortIdentifier.getPortIdentifier( this.robotComPort );
serialPort = (SerialPort)portId.open("SerialCommunicationChannel", 2000);
serialPort.setSerialPortParams(speed, dataBits, stopBit, parity);
in = new BufferedReader( new InputStreamReader( serialPort.getInputStream() ) );
out = serialPort.getOutputStream();
// Se obtiene el protocolo de comunicaci�n
props = new Properties();
props.load( new FileInputStream("config/serialcommchannel.properties") );
FRONT_SENSOR = ("sen(" + props.getProperty("front_sensor_id") + ")\n").getBytes();
RIGHT_SENSOR = ("sen(" + props.getProperty("right_sensor_id") + ")\n").getBytes();
LEFT_SENSOR = ("sen(" + props.getProperty("left_sensor_id") + ")\n" ).getBytes();
SENSOR_THRESHOLD= Integer.valueOf( props.getProperty("sensor_threshold") ).intValue();
ADVANCE = (props.getProperty("pwm_advance") + "\n").getBytes();
BRAKE = (props.getProperty("pwm_brake") + "\n").getBytes();
RIGHT_ROTATE = (props.getProperty("pwm_right_rotate") + "\n").getBytes();
LEFT_ROTATE = (props.getProperty("pwm_left_rotate") + "\n").getBytes();
MOVE_TIME = Integer.valueOf( props.getProperty("pwm_move_time") ).intValue();
robot = Simulation.getCurrent().getRobotByName( robotName );
if ( robot == null )
return Defines.ERROR;
setCommChannelSemaphore( robot.getCommChannelSemaphore() );
}
catch (Exception e)
{
Logger.error( e.getMessage() );
return Defines.ERROR;
}
super.open( null );
return Defines.OK;
}
public Hashtable receive()
{
if ( isClosed() )
return null;
return features.featuresHash;
}
public synchronized void send(String msg)
{
if ( isClosed() ) return;
byte[] auxMsg = null;
try
{
if ( msg != null )
{
String[] fields = msg.split( "\\s*=\\s*" );
if ( fields[0].equalsIgnoreCase( "left" ) )
{
auxMsg = LEFT_ROTATE;
}
else if ( fields[0].equalsIgnoreCase( "right" ) )
{
auxMsg = RIGHT_ROTATE;
}
else if ( fields[0].equalsIgnoreCase( "advance" ) )
{
auxMsg = ADVANCE;
}
if ( auxMsg != null )
{
request( auxMsg );
Thread.sleep( MOVE_TIME );
request( BRAKE );
}
}
}
catch (Exception e)
{
Logger.error( e.getMessage() );
}
}
public boolean test()
{
return true;
}
private Integer getSensorValue(byte[] sensorId)
{
try
{
String response = request( sensorId );
if ( response != null && response.startsWith("[S") )
{
String value = response.substring( response.indexOf(':')+1 , response.length()-1 );
return Integer.valueOf( value );
}
else
{
Logger.error( "La respuesta recibida es incorrecta: " + response );
return null;
}
}
catch(Exception e)
{
Logger.error( e.getMessage() );
return null;
}
}
private synchronized String request(byte[] requestMsg)
{
try
{
Logger.debug( "Request: " + new String(requestMsg) );
out.write( requestMsg );
String response = in.readLine();
Logger.debug( "Response: " + response + "\n" );
return response;
}
catch (Exception e)
{
Logger.error( e.getMessage() );
return null;
}
}
public void run()
{
Sensor sensor;
Integer sensorValue;
RobotState robotState;
double[] pos;
double course;
boolean finished1, finished2, finished3;
RecognizedItems sceneItems;
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() )
{
getCommChannelSemaphore().WAIT();
//////////////////////////////////////////
// //
// Se analiza el SENSOR FRONTAL //
// //
//////////////////////////////////////////
sensor = (ProximitySensor)features.featuresHash.get( FRONT_SENSOR_NAME );
if ( sensor != null )
{
// TODO ELIMINAR HARDCODEO DE AREA Y MASA
if ( frontCam.isCollision(RobotCamColorTrack.TRACK_RAIL, 4000, 50) )
// El sensor est� detectando algo
sensor.setValue( new Boolean(true) );
else
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.");
}
/* EL CODIGO DE SENSORES IZQ Y DER ESTA POR COMPATIBILIDAD HACIA ATR�S, NO HACE NADA*/
sensor = (ProximitySensor)features.featuresHash.get( LEFT_SENSOR_NAME );
if ( sensor != null ) sensor.setValue( new Boolean(false) );
else Logger.error("No se pudo acceder al sensor.");
sensor = (ProximitySensor)features.featuresHash.get( RIGHT_SENSOR_NAME );
if ( sensor != null ) sensor.setValue( new Boolean(false) );
else Logger.error("No se pudo acceder al sensor.");
//////////////////////////////////////////
// Se analiza el SENSOR DE COLOR //
//////////////////////////////////////////
sensor = (ColorSensor)features.featuresHash.get( COLOR_SENSOR_NAME );
// TODO hacer el metodo get current color!!
sensor.setValue( frontCam.getCurrentColor() );
getCommChannelSemaphore().SIGNAL();
// Duerme durante un intervalo
try
{
Thread.sleep( SLEEP_TIME );
}
catch (InterruptedException e) { }
}
serialPort.close();
Logger.info( "El canal de comunicaciones finaliza su ejecuci�n." );
}
public String getXMLConfig(String ident)
{
String xmlConfig = new String();
String tabulator = new String();
tabulator = tabulator.concat(ident);
xmlConfig = xmlConfig.concat(tabulator + "<communicationchannel name=\""+getName()+"\" description=\""+getDescription()+"\">");
tabulator=tabulator.concat("\t");
xmlConfig.concat(tabulator+"<connectionstring>"+connectionString+"</connectionstring>");
tabulator=ident;
xmlConfig=xmlConfig.concat(tabulator+"</communicationchannel>");
return xmlConfig;
}
public void setSpeed(int speed) {
this.speed = speed;
}
public int getSpeed() {
return speed;
}
public void setDataBits(int dataBits) {
this.dataBits = dataBits;
}
public int getDataBits() {
return dataBits;
}
public void setStopBit(int stopBit) {
this.stopBit = stopBit;
}
public int getStopBit() {
return stopBit;
}
public void setParity(int parity) {
this.parity = parity;
}
public int getParity() {
return parity;
}
public void setFrontCameraPort(String text) {
frontCamPort = text;
}
public void setRobotComPort(String text) {
robotComPort = text;
}
}