MAC0318/SonarModel/Main.java

import lejos.geom.Line;
import lejos.geom.Rectangle;
import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.Pose;
import java.io.*;
import java.util.ArrayList;

public class Main {
   private LineMap map;

   public Main() {
      Line[] lines = {
         new Line(0f, 0f, 155f, 0f),
         new Line(155f, 0f, 110f, 135f),
         new Line(110f, 135f, 0f, 132f),
         new Line(0f, 132f, 0f, 0f)
      };
      Rectangle bounds = new Rectangle(0, 0, 1189, 841);
      map = new LineMap(lines, bounds);
   }

   public double expectedSonarRead(Pose p, double angle, int cone) {
      /**************** MODELO DO SONAR *******************/
      Pose tmppose = new Pose(p.getX(), p.getY(), p.getHeading());
      float mindist = Float.POSITIVE_INFINITY;
      for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
         tmppose.setHeading((float) (p.getHeading() - angulo + angle));
         float dist = map.range(tmppose);
         if (dist > 0 && dist < mindist) 
            mindist = dist;
      } 
      return Math.min(mindist, 255);
   }

   public static void fileCalc (String filename) {
      try {
         File file = new File(filename);
         FileReader fileReader = new FileReader(file);
         BufferedReader bufferedReader = new BufferedReader(fileReader);
         String line;
         bufferedReader.readLine();

         ArrayList<Integer> reads = new ArrayList<Integer>();
         ArrayList<Integer> headinng = new ArrayList<Integer>();

         double x = 0;
         double y = 0;
         int head = 0;

         Main m = new Main();

         while ((line = bufferedReader.readLine()) != null) {
            String[] data = line.split(",");

            x = Double.parseDouble(data[0]);
            y = Double.parseDouble(data[1]);
            head = (int)Double.parseDouble(data[2]);
            int sonar_ang = (int)Double.parseDouble(data[3]);
            int read = (int)Double.parseDouble(data[4]);
            // if (read != 255) {
               reads.add(read);
               headinng.add(sonar_ang);
            // }
         }
         // System.out.println("Deletados: "+(181-reads.size()));
         fileReader.close();

         // for (int j = 0; j < reads.size(); j++)
         //    if (headinng.get(j) == 0) {
         //       y = reads.get(j);
         //    }

         // vamos calcular varios cones
         Pose p = new Pose((float)x, (float)y, head);
         System.out.print(p);

         for (int i = 0; i <= 80; i += 2) {
            // para cada posicao
            double sum = 0;
            int count_exclude = 0;
            for (int j = 0; j < reads.size(); j++) {
               double exp = m.expectedSonarRead(p, -headinng.get(j)-90, i);
               double err = Math.abs(reads.get(j)-exp);
               if (err >= 10) {
                  count_exclude++;
                  continue;
               }
               err *= err;
               
                  // System.out.println(headinng.get(j)+" "+reads.get(j)+" "+exp);
               sum += err;
            }
            // System.out.println();
            // System.out.println(count_exclude);
            sum /= (reads.size()-count_exclude);
            System.out.print(","+sum);
         }
         System.out.println();

      } catch (IOException e) {
         e.printStackTrace();
      }
   }

   public static void main(String[] args) {
      String dir = "medidas";
      File folder = new File(dir);
      File[] listOfFiles = folder.listFiles();

      for (int i = 0; i < listOfFiles.length; i++) {
         if (listOfFiles[i].isFile()) {
            // System.out.println("File " + listOfFiles[i].getName());
            fileCalc(dir+"/"+listOfFiles[i].getName());
         }
      }

   }
}