import gnu.io.CommPortIdentifier;
import gnu.io.NoSuchPortException;
import gnu.io.PortInUseException;
import gnu.io.SerialPort;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.OutputStream;
import java.util.ArrayList;
import java.util.InvalidPropertiesFormatException;
import java.util.Properties;
import java.util.StringTokenizer;
public class mandafruta {
private static ArrayList<String> mappingList = new ArrayList<String>();
private static Properties simulationProperties = new Properties();
private static ArrayList<RgbColorRange> railArray = new ArrayList<RgbColorRange>();
private static ArrayList<RgbColorRange> colorArray = new ArrayList<RgbColorRange>();
private static ArrayList<VectorValues> vectorList = new ArrayList<VectorValues>();
private static float bestVectorDistance=Float.MAX_VALUE;
static int MAXIMO_DERECHA_ANCHO = 108;
static int MINIMO_IZQUIERDA_ANCHO = 68;
static int MAXIMO_DERECHA_ANGOSTO = 98;
static int MINIMO_IZQUIERDA_ANGOSTO = 78;
static int MINIMO_AREA_FIN = 8000;
static int MAXIMO_AREA_FIN = 22000;
static int MAXIMO_SUPER_AREA_FIN = 40000;
// static int MINIMO_MASA_FIN = 40;
static int MINIMO_MASA = 20;
static int MINIMO_MASA_CAMBIO_MODO_COLOR = 50;
static int CANTIDAD_VECES_PROMEDIO = 5;
//static SerialCommunicationChannel sc;
static SerialComm sc;
static RobotCamColorTrack prueba;
static OutputStream out;
/**
* @param args
* @throws IOException
* @throws NoSuchPortException
* @throws PortInUseException
*/
public static void main(String[] args) throws Exception {
if(args==null || (args!=null && args.length<1)) {
System.out.println("Es necesario especificar por par�metro el puerto COM a utilizar - Ej: 'java -jar SimpleBot.jar COM4'");
return;
}
CommPortIdentifier portId = CommPortIdentifier.getPortIdentifier( args[0] );
SerialPort sp = (SerialPort)portId.open("SerialCommunicationChannel", 2000);
sp.setSerialPortParams(38400, SerialPort.DATABITS_8, SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
out = sp.getOutputStream();
prueba = new RobotCamColorTrack("3");
if (!prueba.setupCamera()) {
out.close();
sp.close();
return;
}
int remainingMovements = 200;
int direccion = RobotCamColorTrack.NO_MOVE;
int antmove = RobotCamColorTrack.MOVER_ADELANTE;
boolean redPlace = false;
loadProperties();
buildRailArray();
buildColorArray();
int i=0;
while(remainingMovements > 0 && !redPlace) {
int samples = 1;
prueba.grabFrame();
ArrayList<TrackingInformation> tiRails = getRailTrackingInformation(railArray, samples);
ArrayList<TrackingInformation> tiColors = getRailTrackingInformation(colorArray, samples);
TrackingInformation currentRail = calculateBestColor(tiRails);
VectorValues currentVector = calculateBestVector(tiColors);
System.out.println("Movimiento: "+(200-remainingMovements));
System.out.println("===========================");
System.out.println(currentRail.getDescription());
System.out.println(currentRail.toString());
System.out.println("===========================");
System.out.println(currentVector.getDescription());
System.out.println(currentVector.toString());
int railMass = currentRail.getColorMass();
int colorMass = 0;//XtiGray.getColorMass();
direccion = prueba.getTurningDirection(currentRail.getMeanX(),railMass,15,antmove);
antmove = direccion;
System.out.println("IS IN PLACE = "+ isInPlace(currentVector,bestVectorDistance,direccion));
//direccion=RobotCamColorTrack.MOVER_IZQUIERDA;
if (remainingMovements>0)
moverRobot(direccion,(direccion != RobotCamColorTrack.MOVER_ADELANTE) ? '3' : '7');
//
remainingMovements--;
if(colorMass > 255*0.9) {
System.out.println("\n\nLLEGO AL GRIS\n\n");
break;
}
}
out.close();
sp.close();
}
private static boolean isInPlace(VectorValues currentVector, float bestVectorDistance, int direccion) {
if(direccion == RobotCamColorTrack.MOVER_ADELANTE)return false;
if(currentVector.descripcion.equals("Gray"))
return (2.45f < bestVectorDistance && bestVectorDistance < 2.47f);
if(currentVector.descripcion.equals("Yellow"))
return (0.56f < bestVectorDistance && bestVectorDistance < 0.58f);
return false;
}
private static VectorValues calculateBestVector(ArrayList<TrackingInformation> tiColors) {
ArrayList<Float> normColorScore = new ArrayList<Float>();
Float sumArea = 0.0f;
Float sumMass = 0.0f;
Float maxMass = 0.0f;
Float maxArea = 0.0f;
Float bestVector = 0.0f;
int bestIndex = 0;
// max
for (TrackingInformation info : tiColors) {
sumArea += info.getArea();
maxArea= Math.max(maxArea, info.getArea());
sumMass += info.getColorMass();
maxMass = Math.max(maxMass, info.getColorMass());
}
if(maxArea==0.0f) maxArea=1f;
if(maxMass==0.0f) maxMass=1f;
//colores normalizados
for (int i=0; i< tiColors.size(); i++) {
if(i!=tiColors.size()-1) System.out.print(tiColors.get(i).getDescription()+",");
else System.out.println(tiColors.get(i).getDescription());
}
for (int i=0; i< tiColors.size(); i++) {
TrackingInformation info = tiColors.get(i);
Float currentNormArea = (float)info.getArea()/(float)maxArea;
Float currentNormMass = (float)info.getColorMass()/(float)maxMass;
Float currentScore = (float)(currentNormArea*0.0+currentNormMass*1.0);
normColorScore.add(currentScore);
if(i!=tiColors.size()-1) System.out.print((String.format("%1.4f", currentScore)+";").replace(",", ".").replace(";", ","));
else System.out.println(String.format("%1.4f", currentScore).replace(",", "."));
}
System.out.println("VECTORES\n");
bestVector = 1e10f;
//vectores y norma
for (int i=0; i< vectorList.size(); i++) {
VectorValues v = vectorList.get(i);
Float currentNormRail = getEuclidianDistance(normColorScore,v);
System.out.println(vectorList.get(i).getDescription()
+":\t"+currentNormRail);
if(currentNormRail < bestVector) {
bestVector = currentNormRail;
bestIndex = i;
setBestVectorDistance(currentNormRail);
}
}
return vectorList.get(bestIndex);
}
private static void setBestVectorDistance(Float currentVectorNorm) {
bestVectorDistance = currentVectorNorm;
}
private static Float getEuclidianDistance(ArrayList<Float> normColorScore, VectorValues values) {
double v = 0.0d;
float[] currentObs = values.massArray;
for(int i=0; i< Math.min(currentObs.length, normColorScore.size()); i++) {
v += Math.pow((currentObs[i]-normColorScore.get(i)), 2);
//v += Math.abs((currentObs[i]-normColorScore.get(i)));
}
return (float)Math.sqrt(v);
}
private static TrackingInformation calculateBestColor(ArrayList<TrackingInformation> tiColors) {
Float sumArea = 0.0f;
Float sumMass = 0.0f;
Float maxMass = 0.0f;
Float maxArea = 0.0f;
Float bestColor = 0.0f;
int bestIndex = 0;
// max
for (TrackingInformation info : tiColors) {
sumArea += info.getArea();
maxArea= Math.max(maxArea, info.getArea());
sumMass += info.getColorMass();
maxMass = Math.max(maxMass, info.getColorMass());
}
if(maxArea==0.0f) maxArea=1f;
if(maxMass==0.0f) maxMass=1f;
//normalizacion
for (int i=0; i< tiColors.size(); i++) {
TrackingInformation info = tiColors.get(i);
Float currentNormArea = (float)info.getArea()/(float)maxArea;
Float currentNormMass = (float)info.getColorMass()/(float)maxMass;
Float currentBestRail = (float)(currentNormArea*0.0+currentNormMass*1.0);
System.out.println(tiColors.get(i).getDescription()
+"\nNormArea: "+currentNormArea
+"\nNormMass"+currentNormMass
+"\nTotal:"+currentBestRail+"\n\n");
if(currentBestRail > bestColor) {
bestColor = currentBestRail;
bestIndex = i;
}
}
return tiColors.get(bestIndex);
}
private static void loadProperties() throws IOException, InvalidPropertiesFormatException, FileNotFoundException {
simulationProperties.loadFromXML(new FileInputStream("mandafruta.properties.xml"));
}
private static ArrayList<TrackingInformation> getRailTrackingInformation(ArrayList<RgbColorRange> railArray, int samples) {
ArrayList<TrackingInformation> ret = new ArrayList<TrackingInformation>();
for (RgbColorRange r : railArray) {
TrackingInformation t = prueba.addTrackingSamples(r.getLow(),r.getHigh(), samples);
t.setDescription(r.getDescription());
ret.add(t);
}
return ret;
}
private static void buildColorArray() {
for (Object o: simulationProperties.keySet()) {
String color = (String)o;
if(!color.startsWith("color")) continue;
String colorName = color.substring("color".length());
RgbColorRange rgbColor = new RgbColorRange(loadColors(color),colorName);
colorArray.add(rgbColor);
}
String s = (String)simulationProperties.get("ColorMapping");
StringTokenizer tokenizer = new StringTokenizer(s," ,;");
while(tokenizer.hasMoreTokens()) {
mappingList.add((String)tokenizer.nextElement());
}
for (Object o: simulationProperties.keySet()) {
String vector = (String)o;
if(!vector.startsWith("vector")) continue;
String vectorName = vector.substring("vector".length());
VectorValues v = new VectorValues(loadVector(simulationProperties.getProperty(vector)),vectorName);
vectorList.add(v);
}
}
private static void buildRailArray() {
for (Object o: simulationProperties.keySet()) {
String rail = (String)o;
if(!rail.startsWith("rail")) continue;
railArray.add(new RgbColorRange(loadColors(rail),rail.substring("rail".length())));
}
}
private static int[][] loadColors(String color) {
String c = (String)simulationProperties.get(color);
int[][] colorArray = new int[2][3];
StringTokenizer tokenizer = new StringTokenizer(c," ,;");
for (int i = 0; i < 6; i++) {
String value = (String)tokenizer.nextElement();
colorArray[i/3][i % 3]=Integer.parseInt(value);
}
return colorArray;
}
private static float[] loadVector(String values) {
StringTokenizer tokenizer = new StringTokenizer(values," ,;");
float[] colorArray = new float[tokenizer.countTokens()];
for (int i = 0; i < tokenizer.countTokens(); i++) {
String value = (String)tokenizer.nextElement();
colorArray[i]=Float.parseFloat(value);
}
return colorArray;
}
private static void moverRobot(int direccion, char intensidad) throws IOException {
byte salida[] = new byte[3];
switch(direccion) {
case RobotCamColorTrack.MOVER_IZQUIERDA: salida[0] = 'L'; break;
case RobotCamColorTrack.MOVER_DERECHA: salida[0] = 'R'; break;
case RobotCamColorTrack.MOVER_ATRAS: salida[0] = 'B'; break;
case RobotCamColorTrack.MOVER_ADELANTE: salida[0] = 'F'; break;
case RobotCamColorTrack.NO_MOVE: return;
default: return;
}
salida[1] = (byte) intensidad;
salida[2] = '\r';
out.write(salida);
try {
Thread.sleep(300);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}