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.ArrayList;
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.commchannel.serial.frontcam.TrackingInformation;
import components.commchannel.serial.frontcam.VectorValues;
import components.features.sensor.Sensor;
import components.features.sensor.color.ColorSensor;
import components.features.sensor.proximity.ProximitySensor;
import components.gps.imagebased.ImageBasedGPS;
import components.robot.Robot;
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 = 1000; // 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[] ADVANCE;
private byte[] RIGHT_ROTATE;
private byte[] LEFT_ROTATE;
public static 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 static void main(String[] args) {
SerialCommunicationChannel s = new SerialCommunicationChannel("","");
s.frontCamPort="0003";
s.open("a,b");
}
public int open(String connectionStr)
{
String robotName;
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
{
frontCam = new RobotCamColorTrack(this.frontCamPort.substring(3));
if(!frontCam.setupCamera()) {
return Defines.ERROR;
}
portId = CommPortIdentifier.getPortIdentifier( this.robotComPort);
_serialPort = (SerialPort)portId.open("SerialCommunicationChannel", 2000);
_serialPort.setSerialPortParams(38400, SerialPort.DATABITS_8, SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
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") );
ADVANCE = (props.getProperty("pwm_advance") + "\r").getBytes();
RIGHT_ROTATE = (props.getProperty("pwm_right_rotate") + "\r").getBytes();
LEFT_ROTATE = (props.getProperty("pwm_left_rotate") + "\r").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)
{
e.printStackTrace();
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)
{
// log.debug("Llamado desde Thread: "+Thread.currentThread().getName()+"-"+Thread.currentThread().getId());
//
// StackTraceElement[] ste = Thread.currentThread().getStackTrace();
// for (int i = 0; i < ste.length; i++) {
// StackTraceElement element = ste[i];
// log.debug("STACK: "+element.getMethodName()+" - "+element.getLineNumber());
// }
if ( isClosed() ) return;
byte[] auxMsg = null;
try
{
if ( msg != null )
{
String[] fields = msg.split( "\\s*=\\s*" );
if ( fields[0].equalsIgnoreCase( "left" ) )
{
//auxMsg = LEFT_ROTATE;
if(fields.length<2) auxMsg = ("L2\r").getBytes();
else auxMsg = ("L"+fields[1]+"\r").getBytes();
}
else if ( fields[0].equalsIgnoreCase( "right" ) )
{
//auxMsg = RIGHT_ROTATE;
if(fields.length<2) auxMsg = ("R2\r").getBytes();
else auxMsg = ("R"+fields[1]+"\r").getBytes();
}
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 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;
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];
int samples=1;
int direccion = RobotCamColorTrack.NO_MOVE;
int antmove = RobotCamColorTrack.MOVER_ADELANTE;
while ( ! isClosed() )
{
Simulation.getCurrent().getSerialTurn().WAIT();
getCommChannelSemaphore().WAIT();
try{
try {
Thread.sleep(2*SerialCommunicationChannel.MOVE_TIME);
} catch (InterruptedException e) {
e.printStackTrace();
}
long start = System.currentTimeMillis();
///// SE CARGA UNA IMAGEN ////
frontCam.grabFrame();
///// SE OBTIENE INFO DE RIEL Y LUGARES ////
ArrayList<TrackingInformation> tiRails = frontCam.getTrackingInformation(RobotCamColorTrack.TrackType.RAIL_TRACK,samples);
ArrayList<TrackingInformation> tiColors = frontCam.getTrackingInformation(RobotCamColorTrack.TrackType.COLOR_TRACK,samples);
long end = System.currentTimeMillis();
//log.debug("CAM TIME="+((float)(end-start))/1000f);
/// SE DETERMINA EL RIEL Y EL LUGAR ////
TrackingInformation currentRail = frontCam.calculateBestRailColor(tiRails);
VectorValues currentVector = frontCam.calculateBestVector(tiColors);
int railMass = currentRail.getColorMass();
direccion = frontCam.getTurningDirection(currentRail.getMeanX(),railMass,15,antmove);
antmove=direccion;
log.debug("===========================");
log.debug(currentRail.getDescription());
log.debug(currentRail);
log.debug("===========================");
log.debug(currentVector.getDescription().toUpperCase());
log.debug(currentVector.toString());
///// Se graba la recomendacion de nuestros algoritmos acerca de qu� hay que hacer /////
sensor = (ProximitySensor)features.featuresHash.get( MOVE_HINT );
if ( sensor != null )
{
sensor.setValue( new Integer(direccion) );
}
else
{
Logger.error("Oops! No anduvo..");
}
///// Se graba lo que dicen los algoritmos acerca de si llegamos a un lugar /////
sensor = (ProximitySensor)features.featuresHash.get( IN_PLACE );
if ( sensor != null )
{
sensor.setValue( frontCam.isInPlace(currentVector) );
}
else
{
Logger.error("Oops! No anduvo..");
}
///// Se analiza el SENSOR FRONTAL /////
sensor = (ProximitySensor)features.featuresHash.get( FRONT_SENSOR_NAME );
if ( sensor != null )
{
if ( direccion != RobotCamColorTrack.MOVER_ADELANTE )
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.");
}
////// Se analiza el SENSOR DE COLOR /////
sensor = (ColorSensor)features.featuresHash.get( COLOR_SENSOR_NAME );
sensor.setValue( currentVector );
}catch(Exception e){
e.printStackTrace();
}
getCommChannelSemaphore().SIGNAL();
//Duerme durante un intervalo
// try
// {
// Thread.sleep( SLEEP_TIME );
// }
// catch (InterruptedException e) { }
Simulation.getCurrent().getRealRobotTurn().SIGNAL();
}
_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;
}
}