...

HistogramFilter/lejos/Sonar.java

@@ -0,0 +1,177 @@
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+
+import lejos.nxt.Motor;
+import lejos.nxt.NXTRegulatedMotor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.Sound;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.comm.BTConnection;
+import lejos.nxt.comm.Bluetooth;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.localization.*;
+
+class Scanner extends Thread {
+	DataOutputStream output;
+	OdometryPoseProvider provider;
+	boolean run;
+	public boolean scann;
+	public int increment = 5;
+	int position = 0;
+	
+	Scanner (DataOutputStream output, OdometryPoseProvider provider) {
+		super();
+		this.output = output;
+		this.provider = provider;
+		run = true;
+		scann = false;
+	}
+	
+	public void stop () {
+		run = false;
+	}
+	
+	public void run() {
+		NXTRegulatedMotor scannerm = Motor.C;
+		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
+		
+		while (run) {
+			if (scann == false) {
+				position = 0;
+				scannerm.rotateTo(0);
+				scannerm.waitComplete();
+				Thread.yield();
+				continue;
+			}
+
+			scannerm.rotateTo(position*5);
+			System.out.println(position);
+			
+			float distance = sensor.getDistance();			
+			Pose pose = provider.getPose();
+		
+			
+			try {
+				output.write('@');
+				output.write(position);
+				output.writeFloat(distance);
+				output.flush();
+			} catch (IOException e) {
+			}
+		
+			
+			position += increment;
+			if (position == 90 || position == -90)
+				increment *= -1;
+			
+
+		}
+		
+		scannerm.rotateTo(0);
+		scannerm.waitComplete();
+	}
+}
+
+public class Sonar {
+	
+	private static final byte FORWARD = 0;
+	private static final byte STOP = 1;
+	private static final byte EXIT = 2;
+	private static final byte LEFT = 3;
+	private static final byte RIGHT = 4;
+	private static final byte BACKWARD = 5;
+	
+	public static final byte STOPSCANN = 6;
+	public static final byte STARTSCANN = 7;
+	public static final byte MOVE = 8;
+	public static final byte ROTATE = 9;
+	public static final byte SETPOSE = 10;
+	public static final byte SETSCANANGLE = 11;
+	
+	public static void main(String[] args) throws Exception {
+		
+		BTConnection btc = Bluetooth.waitForConnection();
+		//USBConnection btc = USB.waitForConnection();
+		
+		DataInputStream input = btc.openDataInputStream();
+		DataOutputStream output = btc.openDataOutputStream();
+		
+		DifferentialPilot pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A, false); 
+		OdometryPoseProvider provider = new OdometryPoseProvider(pilot);
+		pilot.setRotateSpeed(5);
+		pilot.setTravelSpeed(20);
+		
+		Scanner scan = new Scanner(output, provider);
+		scan.start();
+		int input_byte;
+		boolean run = true;
+		
+		Sound.twoBeeps();
+		
+		while (run) {
+			if (input.available() <= 0) {
+				Thread.yield();
+				continue;
+			}
+			input_byte = input.readByte();
+
+			System.out.println(input_byte);
+			
+			switch (input_byte) {
+				case FORWARD:
+					pilot.forward();
+					break;
+				case STOP:
+					pilot.stop();
+					break;
+				case EXIT:
+					run = false;
+					break;
+				case LEFT:
+					pilot.rotateLeft();
+					break;
+				case RIGHT:
+					pilot.rotateRight();
+					break;
+				case BACKWARD:
+					pilot.backward();
+					break;
+				case STOPSCANN:
+					scan.scann = false;
+					break;
+				case STARTSCANN:
+					scan.position = 90;
+					scan.increment = -Math.abs(scan.increment);
+					scan.scann = true;
+					break;
+				case MOVE:
+					float d = input.readFloat();
+					pilot.travel(d);
+					break;
+				case ROTATE:
+					float r = input.readFloat();
+					pilot.rotate(r);
+					break;
+				case SETPOSE:
+					float x = input.readFloat();
+					float y = input.readFloat();
+					float a = input.readFloat();
+					System.out.println("x: "+x);
+					System.out.println("y: "+y);
+					System.out.println("a: "+a);
+					provider.setPose(new Pose(x,y,a));
+					break;
+				case SETSCANANGLE:
+					int i = input.readByte();
+					System.out.println("ang: "+i);
+					scan.increment = i;
+					break;
+			}
+		}
+		Sound.beep();
+		scan.stop();
+		scan.join();
+	}
+}

HistogramFilter/src/MainProgram.java

@@ -61,18 +61,22 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		splitPane.setDividerLocation((int) (frame.getHeight() / 2));
 		frame.add(splitPane);
 
-		matriz = new ProbabilityMatriz(map.getBoundingRect(), 10, 7);
+		matriz = new ProbabilityMatriz(map.getBoundingRect(), 10, 9);
 		
+		crenca();
+//		showHelp();
+	}
+
+	private void crenca() {
 		for (DiscretePoint p: matriz) {
 //			System.out.println(n(p.midAng()));
-			if (p.z == 0 && p.x == 5 && p.y == 5)
+			if ((p.z == 9 || p.z == 8) && p.x == 1)
 				p.set(1);
 			else
 				p.set(0);
 		}
-		
+		matriz.normalize();
 		imap.showMatriz(matriz);
-//		showHelp();
 	}
 	
 	private double n (double x) {
@@ -133,6 +137,9 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		case 'r':
 			rotateRobot();
 			break;
+		case 'b':
+			crenca();
+			break;
 		case 'a':
 			colectSonar();
 			break;
@@ -196,7 +203,7 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		for (DataRead d : data) {
 			robotData(d);
 		}
-		
+		System.out.println("... "+matriz.sum());
 		imap.repaint();
 	}
 
@@ -269,12 +276,13 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 //			}
 //			newp.set(sum);
 //		}
-//		
-		System.out.println("Finalizado");
+			
 		matriz = newmatriz;
 		imap.setMatriz(matriz);
 		matriz.normalize();
 
+		System.out.println("Finalizado "+matriz.sum());
+
 		scanner.clean();
 		imap.repaint();
 	}
@@ -331,8 +339,8 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 	@Override
 	public void robotData(DataRead data) {
 		// posicao do robo
-		System.out.print("   sang:"+data.getSensorAngle());
-		System.out.println("   distance:"+data.getDistance());
+//		System.out.print("   sang:"+data.getSensorAngle());
+//		System.out.println("   distance:"+data.getDistance());
 		scanner.addRead(data.getDistance(), data.getSensorAngle()-90);
 	}
 

HistogramFilter/src/MapImage.java

@@ -188,10 +188,16 @@ public class MapImage extends JPanel implements MouseWheelListener, MouseListene
     		if (matriz == null) return;
         double w = getWidth()/2.0+centerx;
         double h = getHeight()/2.0+centery;
-        double max = matriz.max();
+//        double max = matriz.max();
+        double n = Math.pow(2, Math.ceil(Math.log(matriz.max())));
     		for (DiscretePoint p: matriz) {
     			
-        		g.setColor(new Color(0.31f, 0.58f, 0.80f, (float)(p.get()/max)));
+//        		g.setColor(new Color(0.31f, 0.58f, 0.80f, (float)(p.get()/max)));
+//        	>>	g.setColor(new Color(0.31f, 0.58f, 0.80f, (float)(Math.pow(p.get(), 0.25))));
+//    			g.setColor(Color.getHSBColor((float) (p.get()*0.9), 1, 1));
+    			
+    			g.setColor(new Color(0.31f, 0.58f, 0.80f, (float) (p.get()/n)));
+    			
     			g.fillArc(
     					(int) (p.bounds().getMinX()*zoom+w),
     					(int) (-p.bounds().getMinY()*zoom+h-matriz.ssize*zoom),

HistogramFilter/src/config/Map.java

@@ -44,14 +44,46 @@ public class Map {
 		// 	new Line(73.0f,40.9f,70.4f,15.0f),
 		// 	new Line(70.4f,15.0f,50.3f,7.6f)
 		// };
+		
+		// mapa norma
+//		Line[] lines2 = {
+//			/* L-shape polygon */
+//			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, 160, 160);
+		
+		// mapa com caixa
+//		Line[] lines2 = {
+//			/* L-shape polygon */
+//			new Line(0f, 0f, 155f, 0f),
+//			new Line(0f, 0f, 0f, 132f),
+//			new Line(0f, 132f, 110f, 132f),
+//			new Line(155f, 0f, 110f, 132f),
+//			
+//			new Line(57f, 46f, 57f, 66f),
+//			new Line(57f, 46f, 77f, 46f),
+//			new Line(77f, 46f, 77f, 66f),
+//			new Line(57f, 66f, 77f, 66f)
+//		};
+//		Rectangle bounds = new Rectangle(0, 0, 160, 135);
+		
 		Line[] lines2 = {
-			/* L-shape polygon */
-			new Line(0f, 0f, 155f, 0f),
-			new Line(155f, 0f, 110f, 135f),
-			new Line(110f, 135f, 0f, 132f),
-			new Line(0f, 132f, 0f, 0f)
+
+			new Line(0f, 0f, 71f, 0f),
+			new Line(71f, 0f, 71f, 132f),
+			new Line(71f, 132f, 0f, 132f),
+			new Line(0f, 132f, 0f ,0f),
+			
+			
+			new Line(52f, 78f, 72f, 78f),
+			new Line(72f, 78f, 72f, 98f),
+			new Line(72f, 98f, 52, 98f),
+			new Line(52f, 98f, 52f, 78f)
 		};
-		Rectangle bounds = new Rectangle(0, 0, 160, 160);
+		Rectangle bounds = new Rectangle(0, 0, 600, 85);
 		LineMap  map = new LineMap(lines2, bounds);
 
 		return map;

HistogramFilter/src/config/Models.java

@@ -10,14 +10,14 @@ import robots.DataRead;
 public class Models {
 	
 	private static boolean USE_TRIANGULAR = false;
-	private static int SONAR_CONE = 10;
-	private static double SONAR_SIGMA = 10;
+	private static int SONAR_CONE = 14;
+	private static double SONAR_SIGMA = 9;
 	
-	private static double ALPHA1 = 0.1;		// desvio padrao do erro no gito
-	private static double ALPHA2 = 0.00015;	// desvio padrao do erro qunado gira proporconal ao deslocalmento
+	private static double ALPHA1 = 0.01;		// desvio padrao do erro no gito
+	private static double ALPHA2 = 0.005;	// desvio padrao do erro qunado gira proporconal ao deslocalmento
 	private static double ALPHA3 = 0.1;		// desvio padrao no erro do deslocamento
 	private static double ALPHA4 = 0.01;    // desvio padrao no erro do deslocamento baseado em quanto rodou (nao importa para o nosso robo)
-	
+
 	LineMap map;
 	public Models(LineMap map) {
 		this.map = map;
@@ -88,7 +88,7 @@ public class Models {
 		double prob = 1.0;
 		for (DataRead dp: data) {
 			// os 90 é para a correcao do mapa
-			float newang = (float) (p.getHeading() + 270 - dp.getSensorAngle() - 90);
+			float newang = (float) (p.getHeading() + 270 + dp.getSensorAngle());
 			Pose pose = new Pose(p.getX(), p.getY(), newang);
 			
 			float mindist = Float.POSITIVE_INFINITY;

HistogramFilter/src/robots/BluetoothRobot.java

@@ -259,7 +259,7 @@ public class BluetoothRobot implements Robot {
 
 	@Override
 	public void rotate(double x) {
-		send(new SendData(ROTATE, (float)x));
+		send(new SendData(ROTATE, (float)-x));
 	}
 
 	@Override