Arrumando falhas

.metadata/.log

@@ -1355,3 +1355,123 @@ java.net.ConnectException: Host is down
 	at org.eclipse.ecf.provider.filetransfer.httpclient4.HttpClientFileSystemBrowser.runRequest(HttpClientFileSystemBrowser.java:263)
 	at org.eclipse.ecf.provider.filetransfer.browse.AbstractFileSystemBrowser$DirectoryJob.run(AbstractFileSystemBrowser.java:69)
 	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.lejos.nxt.ldt 4 0 2017-10-26 22:23:11.729
+!MESSAGE 
+!STACK 0
+java.lang.NullPointerException
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspathEntry(JavaRuntime.java:1164)
+	at org.eclipse.jdt.launching.StandardClasspathProvider.resolveClasspath(StandardClasspathProvider.java:91)
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspath(JavaRuntime.java:1517)
+	at org.eclipse.jdt.launching.AbstractJavaLaunchConfigurationDelegate.getClasspath(AbstractJavaLaunchConfigurationDelegate.java:415)
+	at org.lejos.nxt.ldt.launch.LaunchNXTConfigDelegate.launch(LaunchNXTConfigDelegate.java:89)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:885)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:739)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin.buildAndLaunch(DebugUIPlugin.java:1039)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin$8.run(DebugUIPlugin.java:1256)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.lejos.nxt.ldt 4 0 2017-10-26 22:23:18.642
+!MESSAGE 
+!STACK 0
+java.lang.NullPointerException
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspathEntry(JavaRuntime.java:1164)
+	at org.eclipse.jdt.launching.StandardClasspathProvider.resolveClasspath(StandardClasspathProvider.java:91)
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspath(JavaRuntime.java:1517)
+	at org.eclipse.jdt.launching.AbstractJavaLaunchConfigurationDelegate.getClasspath(AbstractJavaLaunchConfigurationDelegate.java:415)
+	at org.lejos.nxt.ldt.launch.LaunchNXTConfigDelegate.launch(LaunchNXTConfigDelegate.java:89)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:885)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:739)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin.buildAndLaunch(DebugUIPlugin.java:1039)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin$8.run(DebugUIPlugin.java:1256)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.lejos.nxt.ldt 4 0 2017-10-26 22:32:24.378
+!MESSAGE 
+!STACK 0
+java.lang.NullPointerException
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspathEntry(JavaRuntime.java:1164)
+	at org.eclipse.jdt.launching.StandardClasspathProvider.resolveClasspath(StandardClasspathProvider.java:91)
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspath(JavaRuntime.java:1517)
+	at org.eclipse.jdt.launching.AbstractJavaLaunchConfigurationDelegate.getClasspath(AbstractJavaLaunchConfigurationDelegate.java:415)
+	at org.lejos.nxt.ldt.launch.LaunchNXTConfigDelegate.launch(LaunchNXTConfigDelegate.java:89)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:885)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:739)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin.buildAndLaunch(DebugUIPlugin.java:1039)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin$8.run(DebugUIPlugin.java:1256)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.lejos.nxt.ldt 4 0 2017-10-26 22:32:34.078
+!MESSAGE 
+!STACK 0
+java.lang.NullPointerException
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspathEntry(JavaRuntime.java:1164)
+	at org.eclipse.jdt.launching.StandardClasspathProvider.resolveClasspath(StandardClasspathProvider.java:91)
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspath(JavaRuntime.java:1517)
+	at org.eclipse.jdt.launching.AbstractJavaLaunchConfigurationDelegate.getClasspath(AbstractJavaLaunchConfigurationDelegate.java:415)
+	at org.lejos.nxt.ldt.launch.LaunchNXTConfigDelegate.launch(LaunchNXTConfigDelegate.java:89)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:885)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:739)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin.buildAndLaunch(DebugUIPlugin.java:1039)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin$8.run(DebugUIPlugin.java:1256)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.lejos.nxt.ldt 4 0 2017-10-26 22:32:36.110
+!MESSAGE 
+!STACK 0
+java.lang.NullPointerException
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspathEntry(JavaRuntime.java:1164)
+	at org.eclipse.jdt.launching.StandardClasspathProvider.resolveClasspath(StandardClasspathProvider.java:91)
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspath(JavaRuntime.java:1517)
+	at org.eclipse.jdt.launching.AbstractJavaLaunchConfigurationDelegate.getClasspath(AbstractJavaLaunchConfigurationDelegate.java:415)
+	at org.lejos.nxt.ldt.launch.LaunchNXTConfigDelegate.launch(LaunchNXTConfigDelegate.java:89)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:885)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:739)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin.buildAndLaunch(DebugUIPlugin.java:1039)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin$8.run(DebugUIPlugin.java:1256)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.lejos.nxt.ldt 4 0 2017-10-26 22:32:39.269
+!MESSAGE 
+!STACK 0
+java.lang.NullPointerException
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspathEntry(JavaRuntime.java:1164)
+	at org.eclipse.jdt.launching.StandardClasspathProvider.resolveClasspath(StandardClasspathProvider.java:91)
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspath(JavaRuntime.java:1517)
+	at org.eclipse.jdt.launching.AbstractJavaLaunchConfigurationDelegate.getClasspath(AbstractJavaLaunchConfigurationDelegate.java:415)
+	at org.lejos.nxt.ldt.launch.LaunchNXTConfigDelegate.launch(LaunchNXTConfigDelegate.java:89)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:885)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:739)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin.buildAndLaunch(DebugUIPlugin.java:1039)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin$8.run(DebugUIPlugin.java:1256)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.lejos.nxt.ldt 4 0 2017-10-26 22:32:47.844
+!MESSAGE 
+!STACK 0
+java.lang.NullPointerException
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspathEntry(JavaRuntime.java:1164)
+	at org.eclipse.jdt.launching.StandardClasspathProvider.resolveClasspath(StandardClasspathProvider.java:91)
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspath(JavaRuntime.java:1517)
+	at org.eclipse.jdt.launching.AbstractJavaLaunchConfigurationDelegate.getClasspath(AbstractJavaLaunchConfigurationDelegate.java:415)
+	at org.lejos.nxt.ldt.launch.LaunchNXTConfigDelegate.launch(LaunchNXTConfigDelegate.java:89)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:885)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:739)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin.buildAndLaunch(DebugUIPlugin.java:1039)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin$8.run(DebugUIPlugin.java:1256)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.lejos.nxt.ldt 4 0 2017-10-26 22:32:48.703
+!MESSAGE 
+!STACK 0
+java.lang.NullPointerException
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspathEntry(JavaRuntime.java:1164)
+	at org.eclipse.jdt.launching.StandardClasspathProvider.resolveClasspath(StandardClasspathProvider.java:91)
+	at org.eclipse.jdt.launching.JavaRuntime.resolveRuntimeClasspath(JavaRuntime.java:1517)
+	at org.eclipse.jdt.launching.AbstractJavaLaunchConfigurationDelegate.getClasspath(AbstractJavaLaunchConfigurationDelegate.java:415)
+	at org.lejos.nxt.ldt.launch.LaunchNXTConfigDelegate.launch(LaunchNXTConfigDelegate.java:89)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:885)
+	at org.eclipse.debug.internal.core.LaunchConfiguration.launch(LaunchConfiguration.java:739)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin.buildAndLaunch(DebugUIPlugin.java:1039)
+	at org.eclipse.debug.internal.ui.DebugUIPlugin$8.run(DebugUIPlugin.java:1256)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)

.metadata/.plugins/org.eclipse.core.resources/.history/29/e0e78f6fadba001711b8bf632416c20d

@@ -0,0 +1,181 @@
+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.A;
+		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
+		
+		while (run) {
+			if (scann == false)
+				position = 0;
+			
+			if (position == 90 || position == -90)
+				increment *= -1;
+
+			scannerm.rotateTo(position);
+			scannerm.waitComplete();
+
+			if (scann == false)
+				continue;
+			
+			int distance = sensor.getDistance();
+						
+			Pose pose = provider.getPose();
+			float x = pose.getX();
+			float y = pose.getY();
+			float alpha = pose.getHeading();
+			
+			if (distance != 255) {
+				try {
+					output.write('@');
+					output.write(position);
+					output.writeFloat(alpha);
+					output.writeFloat(x);
+					output.write(distance);
+					output.writeFloat(y);
+					output.flush();
+				} catch (IOException e) {
+				}
+			}
+			
+			position += increment;
+		}
+		
+		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, 13.8f, Motor.B, Motor.C, 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(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();
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/4/10dd57dfafba001711b8bf632416c20d

@@ -0,0 +1,181 @@
+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.A;
+		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
+		
+		while (run) {
+			if (scann == false)
+				position = 0;
+			
+			if (position == 90 || position == -90)
+				increment *= -1;
+
+			scannerm.rotateTo(position);
+			scannerm.waitComplete();
+
+			if (scann == false)
+				continue;
+			
+			int distance = sensor.getDistance();
+						
+			Pose pose = provider.getPose();
+			float x = pose.getX();
+			float y = pose.getY();
+			float alpha = pose.getHeading();
+			
+			if (distance != 255) {
+				try {
+					output.write('@');
+					output.write(position);
+					output.writeFloat(alpha);
+					output.writeFloat(x);
+					output.write(distance);
+					output.writeFloat(y);
+					output.flush();
+				} catch (IOException e) {
+				}
+			}
+			
+			position += increment;
+		}
+		
+		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, 13.8f, Motor.B, Motor.C, 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();
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/4/30e07428b3ba001711b8bf632416c20d

@@ -0,0 +1,335 @@
+import java.awt.BorderLayout;
+import java.awt.event.KeyEvent;
+import java.awt.event.KeyListener;
+import java.awt.event.WindowEvent;
+import java.awt.event.WindowListener;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+
+import javax.swing.JFrame;
+import javax.swing.JOptionPane;
+import javax.swing.JPanel;
+import javax.swing.JSplitPane;
+import javax.swing.SwingUtilities;
+
+import config.Map;
+import config.Models;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import robots.BluetoothRobot;
+import robots.DataPose;
+import robots.Robot;
+import robots.RobotReturn;
+import robots.VirtualRobot;
+
+public class MainProgram extends JPanel implements KeyListener, WindowListener, RobotReturn {
+
+	private MapImage imap;
+	private Robot robot;
+	private ScannerImage scanner;
+	private Models smodel;
+
+	public static final byte FORWARD = 0;
+	public static final byte STOP = 1;
+	public static final byte EXIT = 2;
+	public static final byte LEFT = 3;
+	public static final byte RIGHT = 4;
+	public static final byte BACKWARD = 5;
+
+	public MainProgram(LineMap map, Robot robot) {
+		this.robot = robot;
+		JFrame frame = new JFrame("Mapa MAC0318");
+
+		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+		frame.setLayout(new BorderLayout());
+		imap = new MapImage(map);
+		scanner = new ScannerImage(map);
+		smodel = new Models(map);
+		// frame.add(this.map);
+		frame.setSize(800, 800);
+		frame.setVisible(true);
+
+		frame.setFocusable(true);
+		frame.requestFocusInWindow();
+		frame.addKeyListener(this);
+		frame.addWindowListener(this);
+
+		JSplitPane splitPane = new JSplitPane(JSplitPane.HORIZONTAL_SPLIT, scanner, imap);
+		splitPane.setOneTouchExpandable(true);
+		splitPane.setDividerLocation((int) (frame.getHeight() / 2));
+		frame.add(splitPane);
+
+		showHelp();
+	}
+
+	private void showHelp() {
+		String text = "1,2,3 - Change global map view\n";
+		text += "s - Set Pose.\n";
+		text += "l - Show global map trace.\n";
+		text += "c - Clean maps.\n";
+		text += "m - Enter robot movement.\n";
+		text += "r - Enter robo rotation.\n";
+		text += "a - Colect sonar data.\n";
+		text += "z - Make sonar continuous scanner.\n";
+		text += "i - Stop continuous scanner.\n";
+		text += "g - Save global map image.\n";
+		text += "f - Save scanner image.\n";
+		text += "<arrows> - Move robot.\n";
+		text += "h - help.\n";
+		JOptionPane.showMessageDialog(null, text, "HELP", JOptionPane.PLAIN_MESSAGE);
+	}
+
+	public void addPoint(Pose p) {
+		imap.addPoint(p);
+	}
+
+	@Override
+	public void keyPressed(KeyEvent e) {
+
+		char input = e.getKeyChar();
+		switch (input) {
+		case '1':
+			imap.setVisual(0);
+			break;
+		case '2':
+			imap.setVisual(1);
+			break;
+		case '3':
+			imap.setVisual(2);
+			break;
+		case 'l':
+			imap.showLine();
+			break;
+		case 'g':
+			imap.save();
+			break;
+		case 'f':
+			scanner.save();
+			break;
+		case 'c':
+			imap.clean();
+			scanner.clean();
+			break;
+		case 's':
+			setRobotPose();
+			break;
+		case 'm':
+			moveRobot();
+			break;
+		case 'r':
+			rotateRobot();
+			break;
+		case 'a':
+			colectSonar();
+			break;
+		case 'z':
+			robot.scann(this);
+			break;
+		case 'i':
+			robot.stopScann();
+			break;
+		case 'h':
+			showHelp();
+			break;
+		default:
+			break;
+		}
+
+		if (robot == null)
+			return;
+
+		switch (e.getKeyCode()) {
+		case KeyEvent.VK_UP:
+			robot.moveForward();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_DOWN:
+			robot.moveBackward();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_LEFT:
+			robot.moveLeft();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_RIGHT:
+			robot.moveRight();
+			scanner.clean();
+			break;
+		}
+	}
+
+	private void colectSonar() {
+		int interval;
+		try {
+			String rs = JOptionPane.showInputDialog("Interval (degress):");
+			interval = Integer.parseInt(rs);
+		} catch (Exception e) {
+			return;
+		}
+		ArrayList<DataPose> data = robot.scann(-90, 90, interval);
+		if (data == null) return;
+
+		Integer name = new Integer((int) (Math.random() * 1000000));
+		String rand_name = name.toString() + ".txt";
+		FileWriter fileWriter;
+		try {
+			fileWriter = new FileWriter(rand_name);
+		} catch (IOException e) {
+			return;
+		}
+		PrintWriter printWriter = new PrintWriter(fileWriter);
+		printWriter.print("x,y,headinng,sonar_ang,read,expected\n");
+		for (DataPose d : data) {
+			robotData(d);
+
+			double expected = smodel.expectedSonarRead(d.getPose(), d.getSensorAngle()+90);
+			printWriter.print(d.getPose().getX() + ",");
+			printWriter.print(d.getPose().getY() + ",");
+			printWriter.print(d.getPose().getHeading() + ",");
+			printWriter.print(d.getSensorAngle() + ",");
+			printWriter.print(d.getDistance() + ",");
+			printWriter.print(expected + "\n");
+		}
+
+		printWriter.close();
+		JOptionPane.showMessageDialog(null, "Reads saved in " + rand_name);
+	}
+
+	private void rotateRobot() {
+		try {
+			String rs = JOptionPane.showInputDialog("Enter rotation (degress-clockwise):");
+			double r = Double.parseDouble(rs);
+			robot.rotate(r);
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	private void moveRobot() {
+		try {
+			String rs = JOptionPane.showInputDialog("Enter distance (cm):");
+			double r = Double.parseDouble(rs);
+			robot.move(r);
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	private void setRobotPose() {
+		try {
+			String xs = JOptionPane.showInputDialog("Enter x (cm):");
+			if (xs.length() == 0)
+				return;
+			String ys = JOptionPane.showInputDialog("Enter y (cm):");
+			if (ys.length() == 0)
+				return;
+			String as = JOptionPane.showInputDialog("Enter heading (degress):");
+			if (as.length() == 0)
+				return;
+
+			float x = Float.parseFloat(xs);
+			float y = Float.parseFloat(ys);
+			float a = Float.parseFloat(as);
+			robot.setPose(x, y, a);
+			scanner.setPose(new Pose(x, y, a));
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	@Override
+	public void keyReleased(KeyEvent arg0) {
+		if (robot == null)
+			return;
+		robot.stop();
+	}
+
+	@Override
+	public void keyTyped(KeyEvent arg0) {
+	}
+
+	@Override
+	public void windowOpened(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowClosing(WindowEvent e) {
+		System.err.println("Fechando...");
+		if (robot == null)
+			return;
+		robot.disconnect();
+
+	}
+
+	@Override
+	public void windowClosed(WindowEvent e) {
+	}
+
+	@Override
+	public void windowIconified(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowDeiconified(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowActivated(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowDeactivated(WindowEvent e) {
+	}
+
+	@Override
+	public void robotData(DataPose data) {
+		// posicao do robo
+		Pose p = data.getPose();
+		System.out.println(p);
+		imap.addPoint(p);
+		if (data.getDistance() == 255) {
+			scanner.setPose(p);
+			return;
+		}
+
+		// ponto do ultrasonico
+		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading()-90);
+		double dx = Math.cos(sensor_ang) * data.getDistance();
+		double dy = Math.sin(sensor_ang) * data.getDistance();
+		double expected = smodel.expectedSonarRead(p, data.getSensorAngle()-90);
+		imap.addRead(p.getX() + dx, p.getY() + dy);
+		scanner.addRead(p, data.getDistance(), data.getSensorAngle()-90, expected);
+	}
+
+	public static void main(String[] args) {
+
+		LineMap map = Map.makeMap();
+		Robot robotv = new VirtualRobot(map);
+		Robot robotbt = new BluetoothRobot(null);
+
+		Object[] possibleValues = { robotv, robotbt };
+		Object robot = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
+				possibleValues, possibleValues[0]);
+		
+		boolean result = false;
+		if (robot != null)
+			result = ((Robot) robot).connect();
+
+		if (result == false) {
+			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
+			System.exit(ERROR);
+		}
+
+		SwingUtilities.invokeLater(new Runnable() {
+			public void run() {
+				new MainProgram(map, (Robot) robot);
+			}
+		});
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/6e/a01dde71adba001711b8bf632416c20d

@@ -0,0 +1,181 @@
+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.A;
+		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
+		
+		while (run) {
+			if (scann == false)
+				position = 0;
+			
+			if (position == 90 || position == -90)
+				increment *= -1;
+
+			scannerm.rotateTo(position);
+			scannerm.waitComplete();
+
+			if (scann == false)
+				continue;
+			
+			int distance = sensor.getDistance();
+						
+			Pose pose = provider.getPose();
+			float x = pose.getX();
+			float y = pose.getY();
+			float alpha = pose.getHeading();
+			
+			if (distance != 255) {
+				try {
+					output.write('@');
+					output.write(position);
+					output.writeFloat(alpha);
+					output.writeFloat(x);
+					output.write(distance);
+					output.writeFloat(y);
+					output.flush();
+				} catch (IOException e) {
+				}
+			}
+			
+			position += increment;
+		}
+		
+		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, 13.8f, Motor.B, Motor.C, 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(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();
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/6e/b0d22a80b0ba001711b8bf632416c20d

@@ -0,0 +1,333 @@
+package robots;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.concurrent.Semaphore;
+
+import lejos.pc.comm.NXTComm;
+import lejos.pc.comm.NXTCommException;
+import lejos.pc.comm.NXTCommFactory;
+import lejos.pc.comm.NXTInfo;
+import lejos.robotics.navigation.Pose;
+
+public class BluetoothRobot implements Robot {
+	private String name;
+	private NXTComm nxtComm;
+	private RobotReturn rr;
+	private Semaphore readm;
+	private Semaphore sendm;
+	
+	private int lastcmd = -1;
+	
+	public static final byte FORWARD = 0;
+	public static final byte STOP = 1;
+	public static final byte EXIT = 2;
+	public static final byte LEFT = 3;
+	public static final byte RIGHT = 4;
+	public 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 final byte TYPE_INT = 1;
+	public static final byte TYPE_CMD = 2;
+	public static final byte TYPE_POSE = 3;
+	public static final byte TYPE_FLOAT = 4;
+	
+	private static final int scannangle = 5;
+
+	private DataOutputStream output;
+	private DataInputStream input;
+	private Receiver receivethread;
+	private Sender sendthread;
+	
+	private ArrayList<DataPose> reads = null;
+	private ArrayList<SendData> tosend;
+		
+	private class Receiver extends Thread {
+		public boolean run = true;
+		@Override
+		public void run() {
+			int bytes_valiable = -1;
+			
+			while(run) {
+				try {
+					bytes_valiable = input.available();
+				} catch (IOException e1) {
+					// TODO Auto-generated catch block
+					e1.printStackTrace();
+				}
+				if (bytes_valiable >= 0) {
+					try {
+						if (input.readByte() != '@') continue;
+						int angle = input.readByte();
+						float alpha = input.readFloat();
+						float x = input.readFloat();
+						int distance = input.readByte();
+						float y = input.readFloat();
+						DataPose d = new DataPose();
+						d.setDistance(distance);
+						d.setSensorAngle(angle);
+						d.setPose(new Pose(y, -x, alpha));
+						if (rr != null)
+							rr.robotData(d);
+						
+						if (reads != null) {
+							readm.tryAcquire();
+							reads.add(d);
+							readm.release();
+						}
+						
+					} catch (IOException e1) {
+						continue;
+					}
+				}
+			}
+		}
+	}
+
+	private class Sender extends Thread {
+		public boolean run = true;
+		@Override
+		public void run() {		
+			while(run) {
+				SendData value = null;
+				if (sendm.tryAcquire()) {
+					if (tosend.size() > 0) {
+						value = tosend.get(0);
+						tosend.remove(0);
+					}
+					sendm.release();
+				}
+				if (value != null)
+					value.send(output);
+				else
+					try {
+						Thread.sleep(100);
+					} catch (InterruptedException e) {
+
+					}
+			}
+		}
+	}
+	
+	private class SendData {
+		float f;
+		int i;
+		int cmd;
+		Pose p;
+		int type;
+		SendData(int cmd) {
+			this.cmd = cmd;
+			this.type = TYPE_CMD;
+		}
+		SendData(int cmd, int i) {
+			this.cmd = cmd;
+			this.i = i;
+			this.type = TYPE_INT;
+		}
+		SendData(int cmd, float f) {
+			this.cmd = cmd;
+			this.f = f;
+			this.type = TYPE_FLOAT;
+		}
+		SendData(int cmd, Pose p) {
+			this.cmd = cmd;
+			this.p = p;
+			this.type = TYPE_POSE;
+		}
+		public boolean send(DataOutputStream output) {
+			try {
+				switch (type) {
+				case TYPE_CMD:
+					output.write(cmd);
+					break;
+				case TYPE_INT:
+					output.write(cmd);
+					output.write(i);
+					break;
+				case TYPE_FLOAT:
+					output.write(cmd);
+					output.writeFloat(f);
+					break;
+				case TYPE_POSE:
+					output.write(cmd);
+					output.writeFloat(p.getX());
+					output.writeFloat(p.getY());
+					output.writeFloat(p.getHeading());
+					break;
+				default:
+					return false;
+				}
+				output.flush();
+			} catch (IOException e) {
+				return false;
+			}
+			return true;
+		}
+	}
+	
+	public BluetoothRobot (String name) {
+		
+		readm = new Semaphore(1);
+		
+		sendm = new Semaphore(1);
+		tosend = new ArrayList<SendData>();
+		
+		receivethread = new Receiver();
+		sendthread = new Sender();
+		
+		this.name = name;
+	}
+	
+	public void send (SendData sd) {
+		if (sendm.tryAcquire()) {
+			tosend.add(sd);
+			sendm.release();
+		}
+	}
+
+	@Override
+	public void moveForward() {
+		if (lastcmd == FORWARD) return;
+		send(new SendData(FORWARD));
+		lastcmd = FORWARD;
+	}
+
+	@Override
+	public void moveLeft() {
+		if (lastcmd == LEFT) return;
+		send(new SendData(LEFT));
+		lastcmd = LEFT;
+	}
+
+	@Override
+	public void moveRight() {
+		if (lastcmd == RIGHT) return;
+		send(new SendData(RIGHT));
+		lastcmd = RIGHT;
+	}
+
+	@Override
+	public void moveBackward() {
+		if (lastcmd == BACKWARD) return;
+		send(new SendData(BACKWARD));
+		lastcmd = BACKWARD;
+	}
+
+	@Override
+	public boolean connect () {
+		try {
+			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
+			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
+			if (nxtInfo.length == 0) { // failed to find a brick with the ID
+				System.err.println("NO NXT found");
+				return false;
+			}
+			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
+				System.err.println("Failed to open NXT");
+				return false;
+			}
+			
+			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
+			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
+			send(new SendData(SETSCANANGLE, scannangle)); // vai scanear em 5 em 5 graus
+		} catch (NXTCommException e) {
+			return false;
+		}
+		
+		receivethread.start();
+		sendthread.start();
+		
+		return true;	
+	}
+
+	@Override
+	public void stop() {
+		send(new SendData(STOP));
+		lastcmd = -1;
+	}
+
+
+	@Override
+	public void move(double x) {
+		send(new SendData(MOVE, (float)x));
+	}
+
+	@Override
+	public void rotate(double x) {
+		send(new SendData(ROTATE, (float)x));
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		int readsn = Math.abs(ini-end)/interval;
+		send(new SendData(SETSCANANGLE, interval));
+		ArrayList<DataPose> tmp;
+		reads = new ArrayList<DataPose>();
+		scann(null);
+		while(true) {
+			readm.tryAcquire();
+			if (reads.size() >= readsn) break;
+			readm.release();
+			try {
+				Thread.sleep(50);
+			} catch (InterruptedException e) {
+			}
+		}
+		while (readsn < reads.size()) {
+			reads.remove(reads.size()-1);
+		}
+		tmp = reads;
+		reads = null;
+		readm.release();
+		stopScann();
+		send(new SendData(SETSCANANGLE, scannangle));
+		return tmp;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		send(new SendData(STARTSCANN));
+	}
+
+	@Override
+	public void stopScann() {
+		send(new SendData(STOPSCANN));
+	}
+
+	@Override
+	public void disconnect() {
+		send(new SendData(EXIT));
+		if (receivethread == null) return;
+		receivethread.run = false;
+		try {
+			receivethread.join();
+		} catch (InterruptedException e1) {
+			System.out.println("Nao foi possivel finalizar as threads...");
+		}
+		try {
+			nxtComm.close();
+		} catch (IOException e) {
+		}
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		send(new SendData(SETPOSE, new Pose(y, -x, a)));
+	}
+	
+	
+	@Override
+	public String toString() {
+		return "Bluetooth Mestre/Escravo";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/97/8054e968adba001711b8bf632416c20d

@@ -0,0 +1,181 @@
+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.A;
+		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
+		
+		while (run) {
+			if (scann == false)
+				position = 0;
+			
+			if (position == 90 || position == -90)
+				increment *= -1;
+
+			scannerm.rotateTo(position);
+			scannerm.waitComplete();
+
+			if (scann == false)
+				continue;
+			
+			int distance = sensor.getDistance();
+						
+			Pose pose = provider.getPose();
+			float x = pose.getX();
+			float y = pose.getY();
+			float alpha = pose.getHeading();
+			
+			if (distance != 255) {
+				try {
+					output.write('@');
+					output.write(position);
+					output.writeFloat(alpha);
+					output.writeFloat(x);
+					output.write(distance);
+					output.writeFloat(y);
+					output.flush();
+				} catch (IOException e) {
+				}
+			}
+			
+			position += increment;
+		}
+		
+		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, 13.8f, Motor.B, Motor.C, 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 = -5;
+					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();
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/a8/a01ce8bdb2ba001711b8bf632416c20d

@@ -0,0 +1,168 @@
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.image.BufferedImage;
+import java.io.File;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.concurrent.Semaphore;
+
+import javax.imageio.ImageIO;
+import javax.swing.JOptionPane;
+import javax.swing.JPanel;
+
+import lejos.geom.Line;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class ScannerImage extends JPanel {
+	private Pose pose;
+	private LineMap map;
+	private ArrayList<SonarRead> lista_leituras;
+	private Semaphore semaphore;
+
+	class SonarRead {
+		public double distance;
+		public double ang;
+		public double expected;
+
+		SonarRead(double distance, double ang, double expected) {
+			this.ang = ang;
+			this.distance = distance;
+			this.expected = expected;
+		}
+	}
+
+	@Override
+	protected void paintComponent(Graphics g) {
+		int w = this.getWidth() / 2;
+		int h = this.getHeight();
+		int distance = h * 2 / 25;
+		g.setColor(new Color(0f, 0f, 0f, 0.4f));
+		for (int i = 1; i <= 18; i++) {
+			int r = distance * i;
+			g.drawArc(w - r, h - r, 2 * r, 2 * r, 0, 180);
+			g.drawString(new Integer(i * 20).toString(), w - 7, h - r);
+		}
+		g.setColor(new Color(0f, 0f, 0f, 0.5f));
+		for (int i = 1; i < 6; i++) {
+			int lw = (int) (Math.cos(Math.PI / 6.0 * i) * distance * 18);
+			int lh = (int) (Math.sin(Math.PI / 6.0 * i) * distance * 18);
+			g.drawLine(w, h, lw + w, h - lh);
+		}
+		g.setColor(new Color(0f, 0f, 0f, 0.1f));
+		for (int i = 1; i < 12; i++) {
+			int lw = (int) (Math.cos(Math.PI / 12.0 * i) * distance * 18);
+			int lh = (int) (Math.sin(Math.PI / 12.0 * i) * distance * 18);
+			g.drawLine(w, h, lw + w, h - lh);
+		}
+
+		if (semaphore.tryAcquire()) {
+			double d = h * 2.0 / 25.0 / 20.0;
+
+			g.setColor(new Color(0f, 1f, 0f));
+			if (map != null && pose != null) {
+				Line[] lines = map.getLines();
+				for (int i = 0; i < lines.length; i++) {
+					Line l = lines[i];
+					double sin = Math.sin(-Math.toRadians(pose.getHeading()));
+					double cos = Math.cos(-Math.toRadians(pose.getHeading()));
+
+					double x1 = (l.x1 - pose.getX()) * d;
+					double y1 = (l.y1 - pose.getY()) * d;
+
+					double x2 = (l.x2 - pose.getX()) * d;
+					double y2 = (l.y2 - pose.getY()) * d;
+
+					double xx1 = x1 * cos - y1 * sin;
+					double yy1 = y1 * cos + x1 * sin;
+
+					double xx2 = x2 * cos - y2 * sin;
+					double yy2 = y2 * cos + x2 * sin;
+
+					g.drawLine((int) (w + xx1), (int) (h - yy1), (int) (w + xx2), (int) (h - yy2));
+				}
+			}
+
+			drawDots(g, w, h);
+			semaphore.release();
+		}
+	}
+
+	private void drawDots(Graphics g, int w, int h) {
+		int oval_size = 16;
+		int distance = h * 2 / 25;
+		double d = distance / 20.0;
+		double a = -oval_size / 4;
+		double x, y;
+
+		g.setColor(new Color(1f, 0f, 0f));
+		for (SonarRead r : lista_leituras) {
+			x = a + (d * r.distance) * Math.sin(Math.toRadians(r.ang));
+			y = a + (d * r.distance) * Math.cos(Math.toRadians(r.ang));
+			x = w - x;
+			y = h - y;
+			g.setColor(new Color(1f, 0f, 0f));
+			g.fillOval((int) (x - oval_size / 2.0), (int) (y - oval_size / 2.0), oval_size / 2, oval_size / 2);
+
+			x = a + (d * r.expected) * Math.sin(Math.toRadians(r.ang));
+			y = a + (d * r.expected) * Math.cos(Math.toRadians(r.ang));
+			x = w - x;
+			y = h - y;
+			g.setColor(new Color(0f, 0f, 1f));
+			g.fillOval((int) (x - oval_size / 2.0), (int) (y - oval_size / 2.0), oval_size / 2, oval_size / 2);
+		}
+		if (map == null)
+			return;
+	}
+
+	public Pose getPose() {
+		return pose;
+	}
+	
+	public void setPose (Pose p) {
+		pose = p;
+		repaint();
+	}
+	
+	public void addRead(Pose p, double distance, double ang, double expected) {
+		if (semaphore.tryAcquire()) {
+			if (pose == null || lista_leituras.size() >= 180) {
+				pose = new Pose(p.getX(), p.getY(), p.getHeading());
+				lista_leituras.clear();
+			}
+			lista_leituras.add(new SonarRead(distance, ang + 90, expected));
+			semaphore.release();
+		}
+		repaint();
+	}
+
+	public ScannerImage(LineMap map) {
+		this.map = map;
+		semaphore = new Semaphore(1);
+		lista_leituras = new ArrayList<SonarRead>();
+	}
+
+	public void clean() {
+		if (semaphore.tryAcquire()) {
+			lista_leituras.clear();
+			semaphore.release();
+		}
+		repaint();
+	}
+
+    public void save () {
+	    	Integer name = new Integer((int) (Math.random()*1000000));
+	    	BufferedImage imagebuf = new BufferedImage(getWidth(), getHeight(), BufferedImage.TYPE_INT_RGB);
+	    	Graphics g = imagebuf.createGraphics();
+	    	g.fillRect(0, 0, imagebuf.getWidth(), imagebuf.getHeight());
+	    	print(g);
+	    	try {
+			ImageIO.write(imagebuf, "png",  new File(name.toString()+".png"));
+			JOptionPane.showMessageDialog(null, "Image saved.");
+		} catch (IOException e) {
+			e.printStackTrace();
+			JOptionPane.showMessageDialog(null, "Image not saved.");
+		}
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/b9/a0aa18b3b3ba001711b8bf632416c20d

@@ -0,0 +1,337 @@
+import java.awt.BorderLayout;
+import java.awt.event.KeyEvent;
+import java.awt.event.KeyListener;
+import java.awt.event.WindowEvent;
+import java.awt.event.WindowListener;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+
+import javax.swing.JFrame;
+import javax.swing.JOptionPane;
+import javax.swing.JPanel;
+import javax.swing.JSplitPane;
+import javax.swing.SwingUtilities;
+
+import config.Map;
+import config.Models;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import robots.BluetoothRobot;
+import robots.DataPose;
+import robots.Robot;
+import robots.RobotReturn;
+import robots.VirtualRobot;
+
+public class MainProgram extends JPanel implements KeyListener, WindowListener, RobotReturn {
+
+	private MapImage imap;
+	private Robot robot;
+	private ScannerImage scanner;
+	private Models smodel;
+
+	public static final byte FORWARD = 0;
+	public static final byte STOP = 1;
+	public static final byte EXIT = 2;
+	public static final byte LEFT = 3;
+	public static final byte RIGHT = 4;
+	public static final byte BACKWARD = 5;
+
+	public MainProgram(LineMap map, Robot robot) {
+		this.robot = robot;
+		JFrame frame = new JFrame("Mapa MAC0318");
+
+		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+		frame.setLayout(new BorderLayout());
+		imap = new MapImage(map);
+		scanner = new ScannerImage(map);
+		smodel = new Models(map);
+		// frame.add(this.map);
+		frame.setSize(800, 800);
+		frame.setVisible(true);
+
+		frame.setFocusable(true);
+		frame.requestFocusInWindow();
+		frame.addKeyListener(this);
+		frame.addWindowListener(this);
+
+		JSplitPane splitPane = new JSplitPane(JSplitPane.HORIZONTAL_SPLIT, scanner, imap);
+		splitPane.setOneTouchExpandable(true);
+		splitPane.setDividerLocation((int) (frame.getHeight() / 2));
+		frame.add(splitPane);
+
+		showHelp();
+	}
+
+	private void showHelp() {
+		String text = "1,2,3 - Change global map view\n";
+		text += "s - Set Pose.\n";
+		text += "l - Show global map trace.\n";
+		text += "c - Clean maps.\n";
+		text += "m - Enter robot movement.\n";
+		text += "r - Enter robo rotation.\n";
+		text += "a - Colect sonar data.\n";
+		text += "z - Make sonar continuous scanner.\n";
+		text += "i - Stop continuous scanner.\n";
+		text += "g - Save global map image.\n";
+		text += "f - Save scanner image.\n";
+		text += "<arrows> - Move robot.\n";
+		text += "h - help.\n";
+		JOptionPane.showMessageDialog(null, text, "HELP", JOptionPane.PLAIN_MESSAGE);
+	}
+
+	public void addPoint(Pose p) {
+		imap.addPoint(p);
+	}
+
+	@Override
+	public void keyPressed(KeyEvent e) {
+
+		char input = e.getKeyChar();
+		switch (input) {
+		case '1':
+			imap.setVisual(0);
+			break;
+		case '2':
+			imap.setVisual(1);
+			break;
+		case '3':
+			imap.setVisual(2);
+			break;
+		case 'l':
+			imap.showLine();
+			break;
+		case 'g':
+			imap.save();
+			break;
+		case 'f':
+			scanner.save();
+			break;
+		case 'c':
+			imap.clean();
+			scanner.clean();
+			break;
+		case 's':
+			setRobotPose();
+			break;
+		case 'm':
+			moveRobot();
+			break;
+		case 'r':
+			rotateRobot();
+			break;
+		case 'a':
+			colectSonar();
+			break;
+		case 'z':
+			robot.scann(this);
+			break;
+		case 'i':
+			robot.stopScann();
+			break;
+		case 'h':
+			showHelp();
+			break;
+		default:
+			break;
+		}
+
+		if (robot == null)
+			return;
+
+		switch (e.getKeyCode()) {
+		case KeyEvent.VK_UP:
+			robot.moveForward();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_DOWN:
+			robot.moveBackward();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_LEFT:
+			robot.moveLeft();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_RIGHT:
+			robot.moveRight();
+			scanner.clean();
+			break;
+		}
+	}
+
+	private void colectSonar() {
+		int interval;
+		try {
+			String rs = JOptionPane.showInputDialog("Interval (degress):");
+			interval = Integer.parseInt(rs);
+		} catch (Exception e) {
+			return;
+		}
+		ArrayList<DataPose> data = robot.scann(-90, 90, interval);
+		if (data == null) return;
+
+		Integer name = new Integer((int) (Math.random() * 1000000));
+		String rand_name = name.toString() + ".txt";
+		FileWriter fileWriter;
+		try {
+			fileWriter = new FileWriter(rand_name);
+		} catch (IOException e) {
+			return;
+		}
+
+		PrintWriter printWriter = new PrintWriter(fileWriter);
+		printWriter.print("x,y,headinng,sonar_ang,read,expected\n");
+		for (DataPose d : data) {
+			robotData(d);
+
+			double expected = smodel.expectedSonarRead(d.getPose(), d.getSensorAngle()+90);
+			printWriter.print(d.getPose().getX() + ",");
+			printWriter.print(d.getPose().getY() + ",");
+			printWriter.print(d.getPose().getHeading() + ",");
+			printWriter.print(d.getSensorAngle() + ",");
+			printWriter.print(d.getDistance() + ",");
+			printWriter.print(expected + "\n");
+		}
+
+		printWriter.close();
+		JOptionPane.showMessageDialog(null, "Reads saved in " + rand_name);
+	}
+
+	private void rotateRobot() {
+		try {
+			String rs = JOptionPane.showInputDialog("Enter rotation (degress-clockwise):");
+			double r = Double.parseDouble(rs);
+			robot.rotate(r);
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	private void moveRobot() {
+		try {
+			String rs = JOptionPane.showInputDialog("Enter distance (cm):");
+			double r = Double.parseDouble(rs);
+			robot.move(r);
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	private void setRobotPose() {
+		try {
+			String xs = JOptionPane.showInputDialog("Enter x (cm):");
+			if (xs.length() == 0)
+				return;
+			String ys = JOptionPane.showInputDialog("Enter y (cm):");
+			if (ys.length() == 0)
+				return;
+			String as = JOptionPane.showInputDialog("Enter heading (degress):");
+			if (as.length() == 0)
+				return;
+
+			float x = Float.parseFloat(xs);
+			float y = Float.parseFloat(ys);
+			float a = Float.parseFloat(as);
+			robot.setPose(x, y, a);
+			scanner.setPose(new Pose(x, y, a));
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	@Override
+	public void keyReleased(KeyEvent arg0) {
+		if (robot == null)
+			return;
+		robot.stop();
+	}
+
+	@Override
+	public void keyTyped(KeyEvent arg0) {
+	}
+
+	@Override
+	public void windowOpened(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowClosing(WindowEvent e) {
+		System.err.println("Fechando...");
+		if (robot == null)
+			return;
+		robot.disconnect();
+
+	}
+
+	@Override
+	public void windowClosed(WindowEvent e) {
+	}
+
+	@Override
+	public void windowIconified(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowDeiconified(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowActivated(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowDeactivated(WindowEvent e) {
+	}
+
+	@Override
+	public void robotData(DataPose data) {
+		// posicao do robo
+		Pose p = data.getPose();
+		System.out.println(p);
+		imap.addPoint(p);
+		if (data.getDistance() == 255) {
+			scanner.setPose(p);
+			return;
+		}
+
+		// ponto do ultrasonico
+		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading()-90);
+		double dx = Math.cos(sensor_ang) * data.getDistance();
+		double dy = Math.sin(sensor_ang) * data.getDistance();
+		double expected = smodel.expectedSonarRead(p, data.getSensorAngle()-90);
+		imap.addRead(p.getX() + dx, p.getY() + dy);
+		scanner.setPose(p);
+		scanner.addRead(p, data.getDistance(), data.getSensorAngle()-90, expected);
+	}
+
+	public static void main(String[] args) {
+
+		LineMap map = Map.makeMap();
+		Robot robotv = new VirtualRobot(map);
+		Robot robotbt = new BluetoothRobot(null);
+
+		Object[] possibleValues = { robotv, robotbt };
+		Object robot = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
+				possibleValues, possibleValues[0]);
+		
+		boolean result = false;
+		if (robot != null)
+			result = ((Robot) robot).connect();
+
+		if (result == false) {
+			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
+			System.exit(ERROR);
+		}
+
+		SwingUtilities.invokeLater(new Runnable() {
+			public void run() {
+				new MainProgram(map, (Robot) robot);
+			}
+		});
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/bd/a005a8aab3ba001711b8bf632416c20d

@@ -0,0 +1,336 @@
+import java.awt.BorderLayout;
+import java.awt.event.KeyEvent;
+import java.awt.event.KeyListener;
+import java.awt.event.WindowEvent;
+import java.awt.event.WindowListener;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+
+import javax.swing.JFrame;
+import javax.swing.JOptionPane;
+import javax.swing.JPanel;
+import javax.swing.JSplitPane;
+import javax.swing.SwingUtilities;
+
+import config.Map;
+import config.Models;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import robots.BluetoothRobot;
+import robots.DataPose;
+import robots.Robot;
+import robots.RobotReturn;
+import robots.VirtualRobot;
+
+public class MainProgram extends JPanel implements KeyListener, WindowListener, RobotReturn {
+
+	private MapImage imap;
+	private Robot robot;
+	private ScannerImage scanner;
+	private Models smodel;
+
+	public static final byte FORWARD = 0;
+	public static final byte STOP = 1;
+	public static final byte EXIT = 2;
+	public static final byte LEFT = 3;
+	public static final byte RIGHT = 4;
+	public static final byte BACKWARD = 5;
+
+	public MainProgram(LineMap map, Robot robot) {
+		this.robot = robot;
+		JFrame frame = new JFrame("Mapa MAC0318");
+
+		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+		frame.setLayout(new BorderLayout());
+		imap = new MapImage(map);
+		scanner = new ScannerImage(map);
+		smodel = new Models(map);
+		// frame.add(this.map);
+		frame.setSize(800, 800);
+		frame.setVisible(true);
+
+		frame.setFocusable(true);
+		frame.requestFocusInWindow();
+		frame.addKeyListener(this);
+		frame.addWindowListener(this);
+
+		JSplitPane splitPane = new JSplitPane(JSplitPane.HORIZONTAL_SPLIT, scanner, imap);
+		splitPane.setOneTouchExpandable(true);
+		splitPane.setDividerLocation((int) (frame.getHeight() / 2));
+		frame.add(splitPane);
+
+		showHelp();
+	}
+
+	private void showHelp() {
+		String text = "1,2,3 - Change global map view\n";
+		text += "s - Set Pose.\n";
+		text += "l - Show global map trace.\n";
+		text += "c - Clean maps.\n";
+		text += "m - Enter robot movement.\n";
+		text += "r - Enter robo rotation.\n";
+		text += "a - Colect sonar data.\n";
+		text += "z - Make sonar continuous scanner.\n";
+		text += "i - Stop continuous scanner.\n";
+		text += "g - Save global map image.\n";
+		text += "f - Save scanner image.\n";
+		text += "<arrows> - Move robot.\n";
+		text += "h - help.\n";
+		JOptionPane.showMessageDialog(null, text, "HELP", JOptionPane.PLAIN_MESSAGE);
+	}
+
+	public void addPoint(Pose p) {
+		imap.addPoint(p);
+	}
+
+	@Override
+	public void keyPressed(KeyEvent e) {
+
+		char input = e.getKeyChar();
+		switch (input) {
+		case '1':
+			imap.setVisual(0);
+			break;
+		case '2':
+			imap.setVisual(1);
+			break;
+		case '3':
+			imap.setVisual(2);
+			break;
+		case 'l':
+			imap.showLine();
+			break;
+		case 'g':
+			imap.save();
+			break;
+		case 'f':
+			scanner.save();
+			break;
+		case 'c':
+			imap.clean();
+			scanner.clean();
+			break;
+		case 's':
+			setRobotPose();
+			break;
+		case 'm':
+			moveRobot();
+			break;
+		case 'r':
+			rotateRobot();
+			break;
+		case 'a':
+			colectSonar();
+			break;
+		case 'z':
+			robot.scann(this);
+			break;
+		case 'i':
+			robot.stopScann();
+			break;
+		case 'h':
+			showHelp();
+			break;
+		default:
+			break;
+		}
+
+		if (robot == null)
+			return;
+
+		switch (e.getKeyCode()) {
+		case KeyEvent.VK_UP:
+			robot.moveForward();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_DOWN:
+			robot.moveBackward();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_LEFT:
+			robot.moveLeft();
+			scanner.clean();
+			break;
+		case KeyEvent.VK_RIGHT:
+			robot.moveRight();
+			scanner.clean();
+			break;
+		}
+	}
+
+	private void colectSonar() {
+		int interval;
+		try {
+			String rs = JOptionPane.showInputDialog("Interval (degress):");
+			interval = Integer.parseInt(rs);
+		} catch (Exception e) {
+			return;
+		}
+		ArrayList<DataPose> data = robot.scann(-90, 90, interval);
+		if (data == null) return;
+
+		Integer name = new Integer((int) (Math.random() * 1000000));
+		String rand_name = name.toString() + ".txt";
+		FileWriter fileWriter;
+		try {
+			fileWriter = new FileWriter(rand_name);
+		} catch (IOException e) {
+			return;
+		}
+
+		PrintWriter printWriter = new PrintWriter(fileWriter);
+		printWriter.print("x,y,headinng,sonar_ang,read,expected\n");
+		for (DataPose d : data) {
+			robotData(d);
+
+			double expected = smodel.expectedSonarRead(d.getPose(), d.getSensorAngle()+90);
+			printWriter.print(d.getPose().getX() + ",");
+			printWriter.print(d.getPose().getY() + ",");
+			printWriter.print(d.getPose().getHeading() + ",");
+			printWriter.print(d.getSensorAngle() + ",");
+			printWriter.print(d.getDistance() + ",");
+			printWriter.print(expected + "\n");
+		}
+
+		printWriter.close();
+		JOptionPane.showMessageDialog(null, "Reads saved in " + rand_name);
+	}
+
+	private void rotateRobot() {
+		try {
+			String rs = JOptionPane.showInputDialog("Enter rotation (degress-clockwise):");
+			double r = Double.parseDouble(rs);
+			robot.rotate(r);
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	private void moveRobot() {
+		try {
+			String rs = JOptionPane.showInputDialog("Enter distance (cm):");
+			double r = Double.parseDouble(rs);
+			robot.move(r);
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	private void setRobotPose() {
+		try {
+			String xs = JOptionPane.showInputDialog("Enter x (cm):");
+			if (xs.length() == 0)
+				return;
+			String ys = JOptionPane.showInputDialog("Enter y (cm):");
+			if (ys.length() == 0)
+				return;
+			String as = JOptionPane.showInputDialog("Enter heading (degress):");
+			if (as.length() == 0)
+				return;
+
+			float x = Float.parseFloat(xs);
+			float y = Float.parseFloat(ys);
+			float a = Float.parseFloat(as);
+			robot.setPose(x, y, a);
+			scanner.setPose(new Pose(x, y, a));
+			scanner.clean();
+		} catch (Exception e) {
+		}
+	}
+
+	@Override
+	public void keyReleased(KeyEvent arg0) {
+		if (robot == null)
+			return;
+		robot.stop();
+	}
+
+	@Override
+	public void keyTyped(KeyEvent arg0) {
+	}
+
+	@Override
+	public void windowOpened(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowClosing(WindowEvent e) {
+		System.err.println("Fechando...");
+		if (robot == null)
+			return;
+		robot.disconnect();
+
+	}
+
+	@Override
+	public void windowClosed(WindowEvent e) {
+	}
+
+	@Override
+	public void windowIconified(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowDeiconified(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowActivated(WindowEvent e) {
+
+	}
+
+	@Override
+	public void windowDeactivated(WindowEvent e) {
+	}
+
+	@Override
+	public void robotData(DataPose data) {
+		// posicao do robo
+		Pose p = data.getPose();
+		System.out.println(p);
+		imap.addPoint(p);
+		if (data.getDistance() == 255) {
+			scanner.setPose(p);
+			return;
+		}
+
+		// ponto do ultrasonico
+		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading()-90);
+		double dx = Math.cos(sensor_ang) * data.getDistance();
+		double dy = Math.sin(sensor_ang) * data.getDistance();
+		double expected = smodel.expectedSonarRead(p, data.getSensorAngle()-90);
+		imap.addRead(p.getX() + dx, p.getY() + dy);
+		scanner.addRead(p, data.getDistance(), data.getSensorAngle()-90, expected);
+	}
+
+	public static void main(String[] args) {
+
+		LineMap map = Map.makeMap();
+		Robot robotv = new VirtualRobot(map);
+		Robot robotbt = new BluetoothRobot(null);
+
+		Object[] possibleValues = { robotv, robotbt };
+		Object robot = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
+				possibleValues, possibleValues[0]);
+		
+		boolean result = false;
+		if (robot != null)
+			result = ((Robot) robot).connect();
+
+		if (result == false) {
+			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
+			System.exit(ERROR);
+		}
+
+		SwingUtilities.invokeLater(new Runnable() {
+			public void run() {
+				new MainProgram(map, (Robot) robot);
+			}
+		});
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/c7/700e3960adba001711b8bf632416c20d

@@ -0,0 +1,180 @@
+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.A;
+		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
+		
+		while (run) {
+			if (scann == false)
+				position = 0;
+			
+			if (position == 90 || position == -90)
+				increment *= -1;
+
+			scannerm.rotateTo(position);
+			scannerm.waitComplete();
+
+			if (scann == false)
+				continue;
+			
+			int distance = sensor.getDistance();
+						
+			Pose pose = provider.getPose();
+			float x = pose.getX();
+			float y = pose.getY();
+			float alpha = pose.getHeading();
+			
+			if (distance != 255) {
+				try {
+					output.write('@');
+					output.write(position);
+					output.writeFloat(alpha);
+					output.writeFloat(x);
+					output.write(distance);
+					output.writeFloat(y);
+					output.flush();
+				} catch (IOException e) {
+				}
+			}
+			
+			position += increment;
+		}
+		
+		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, 13.8f, Motor.B, Motor.C, 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.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();
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/cf/70a1d89cb1ba001711b8bf632416c20d

@@ -0,0 +1,168 @@
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.image.BufferedImage;
+import java.io.File;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.concurrent.Semaphore;
+
+import javax.imageio.ImageIO;
+import javax.swing.JOptionPane;
+import javax.swing.JPanel;
+
+import lejos.geom.Line;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class ScannerImage extends JPanel {
+	private Pose pose;
+	private LineMap map;
+	private ArrayList<SonarRead> lista_leituras;
+	private Semaphore semaphore;
+
+	class SonarRead {
+		public double distance;
+		public double ang;
+		public double expected;
+
+		SonarRead(double distance, double ang, double expected) {
+			this.ang = ang;
+			this.distance = distance;
+			this.expected = expected;
+		}
+	}
+
+	@Override
+	protected void paintComponent(Graphics g) {
+		int w = this.getWidth() / 2;
+		int h = this.getHeight();
+		int distance = h * 2 / 25;
+		g.setColor(new Color(0f, 0f, 0f, 0.4f));
+		for (int i = 1; i <= 18; i++) {
+			int r = distance * i;
+			g.drawArc(w - r, h - r, 2 * r, 2 * r, 0, 180);
+			g.drawString(new Integer(i * 20).toString(), w - 7, h - r);
+		}
+		g.setColor(new Color(0f, 0f, 0f, 0.5f));
+		for (int i = 1; i < 6; i++) {
+			int lw = (int) (Math.cos(Math.PI / 6.0 * i) * distance * 18);
+			int lh = (int) (Math.sin(Math.PI / 6.0 * i) * distance * 18);
+			g.drawLine(w, h, lw + w, h - lh);
+		}
+		g.setColor(new Color(0f, 0f, 0f, 0.1f));
+		for (int i = 1; i < 12; i++) {
+			int lw = (int) (Math.cos(Math.PI / 12.0 * i) * distance * 18);
+			int lh = (int) (Math.sin(Math.PI / 12.0 * i) * distance * 18);
+			g.drawLine(w, h, lw + w, h - lh);
+		}
+
+		if (semaphore.tryAcquire()) {
+			double d = h * 2.0 / 25.0 / 20.0;
+
+			g.setColor(new Color(0f, 1f, 0f));
+			if (map != null && pose != null) {
+				Line[] lines = map.getLines();
+				for (int i = 0; i < lines.length; i++) {
+					Line l = lines[i];
+					double sin = Math.sin(-Math.toRadians(pose.getHeading() - 180));
+					double cos = Math.cos(-Math.toRadians(pose.getHeading() - 180));
+
+					double x1 = (l.x1 - pose.getX()) * d;
+					double y1 = (l.y1 - pose.getY()) * d;
+
+					double x2 = (l.x2 - pose.getX()) * d;
+					double y2 = (l.y2 - pose.getY()) * d;
+
+					double xx1 = x1 * cos - y1 * sin;
+					double yy1 = y1 * cos + x1 * sin;
+
+					double xx2 = x2 * cos - y2 * sin;
+					double yy2 = y2 * cos + x2 * sin;
+
+					g.drawLine((int) (w + xx1), (int) (h - yy1), (int) (w + xx2), (int) (h - yy2));
+				}
+			}
+
+			drawDots(g, w, h);
+			semaphore.release();
+		}
+	}
+
+	private void drawDots(Graphics g, int w, int h) {
+		int oval_size = 16;
+		int distance = h * 2 / 25;
+		double d = distance / 20.0;
+		double a = -oval_size / 4;
+		double x, y;
+
+		g.setColor(new Color(1f, 0f, 0f));
+		for (SonarRead r : lista_leituras) {
+			x = a + (d * r.distance) * Math.sin(Math.toRadians(r.ang));
+			y = a + (d * r.distance) * Math.cos(Math.toRadians(r.ang));
+			x = w - x;
+			y = h - y;
+			g.setColor(new Color(1f, 0f, 0f));
+			g.fillOval((int) (x - oval_size / 2.0), (int) (y - oval_size / 2.0), oval_size / 2, oval_size / 2);
+
+			x = a + (d * r.expected) * Math.sin(Math.toRadians(r.ang));
+			y = a + (d * r.expected) * Math.cos(Math.toRadians(r.ang));
+			x = w - x;
+			y = h - y;
+			g.setColor(new Color(0f, 0f, 1f));
+			g.fillOval((int) (x - oval_size / 2.0), (int) (y - oval_size / 2.0), oval_size / 2, oval_size / 2);
+		}
+		if (map == null)
+			return;
+	}
+
+	public Pose getPose() {
+		return pose;
+	}
+	
+	public void setPose (Pose p) {
+		pose = p;
+		repaint();
+	}
+	
+	public void addRead(Pose p, double distance, double ang, double expected) {
+		if (semaphore.tryAcquire()) {
+			if (pose == null || lista_leituras.size() >= 180) {
+				pose = new Pose(p.getX(), p.getY(), p.getHeading());
+				lista_leituras.clear();
+			}
+			lista_leituras.add(new SonarRead(distance, ang + 90, expected));
+			semaphore.release();
+		}
+		repaint();
+	}
+
+	public ScannerImage(LineMap map) {
+		this.map = map;
+		semaphore = new Semaphore(1);
+		lista_leituras = new ArrayList<SonarRead>();
+	}
+
+	public void clean() {
+		if (semaphore.tryAcquire()) {
+			lista_leituras.clear();
+			semaphore.release();
+		}
+		repaint();
+	}
+
+    public void save () {
+	    	Integer name = new Integer((int) (Math.random()*1000000));
+	    	BufferedImage imagebuf = new BufferedImage(getWidth(), getHeight(), BufferedImage.TYPE_INT_RGB);
+	    	Graphics g = imagebuf.createGraphics();
+	    	g.fillRect(0, 0, imagebuf.getWidth(), imagebuf.getHeight());
+	    	print(g);
+	    	try {
+			ImageIO.write(imagebuf, "png",  new File(name.toString()+".png"));
+			JOptionPane.showMessageDialog(null, "Image saved.");
+		} catch (IOException e) {
+			e.printStackTrace();
+			JOptionPane.showMessageDialog(null, "Image not saved.");
+		}
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/df/901dde21aeba001711b8bf632416c20d

@@ -0,0 +1,181 @@
+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.A;
+		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
+		
+		while (run) {
+			if (scann == false)
+				position = 0;
+			
+			if (position == 90 || position == -90)
+				increment *= -1;
+
+			scannerm.rotateTo(position);
+			scannerm.waitComplete();
+
+			if (scann == false)
+				continue;
+			
+			int distance = sensor.getDistance();
+						
+			Pose pose = provider.getPose();
+			float x = pose.getX();
+			float y = pose.getY();
+			float alpha = pose.getHeading();
+			
+			if (distance != 255) {
+				try {
+					output.write('@');
+					output.write(position);
+					output.writeFloat(alpha);
+					output.writeFloat(x);
+					output.write(distance);
+					output.writeFloat(y);
+					output.flush();
+				} catch (IOException e) {
+				}
+			}
+			
+			position += increment;
+		}
+		
+		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, 13.8f, Motor.B, Motor.C, 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();
+	}
+}

Sonar/507475.txt

@@ -0,0 +1,181 @@
+x,y,headinng,sonar_ang,read,expected
+0.0,-0.0,0.0,90.0,27.0,Infinity
+0.0,-0.0,0.0,89.0,28.0,Infinity
+0.0,-0.0,0.0,88.0,29.0,Infinity
+0.0,-0.0,0.0,87.0,79.0,Infinity
+0.0,-0.0,0.0,86.0,79.0,Infinity
+0.0,-0.0,0.0,85.0,29.0,Infinity
+0.0,-0.0,0.0,84.0,30.0,Infinity
+0.0,-0.0,0.0,83.0,30.0,Infinity
+0.0,-0.0,0.0,82.0,89.0,Infinity
+0.0,-0.0,0.0,81.0,90.0,Infinity
+0.0,-0.0,0.0,80.0,91.0,Infinity
+0.0,-0.0,0.0,79.0,91.0,Infinity
+0.0,-0.0,0.0,78.0,91.0,Infinity
+0.0,-0.0,0.0,77.0,87.0,Infinity
+0.0,-0.0,0.0,76.0,92.0,Infinity
+0.0,-0.0,0.0,75.0,85.0,Infinity
+0.0,-0.0,0.0,74.0,85.0,Infinity
+0.0,-0.0,0.0,73.0,84.0,Infinity
+0.0,-0.0,0.0,72.0,84.0,Infinity
+0.0,-0.0,0.0,71.0,84.0,Infinity
+0.0,-0.0,0.0,70.0,84.0,Infinity
+0.0,-0.0,0.0,69.0,84.0,Infinity
+0.0,-0.0,0.0,68.0,83.0,Infinity
+0.0,-0.0,0.0,67.0,83.0,Infinity
+0.0,-0.0,0.0,66.0,83.0,Infinity
+0.0,-0.0,0.0,65.0,82.0,Infinity
+0.0,-0.0,0.0,64.0,83.0,Infinity
+0.0,-0.0,0.0,63.0,83.0,Infinity
+0.0,-0.0,0.0,62.0,83.0,Infinity
+0.0,-0.0,0.0,61.0,82.0,Infinity
+0.0,-0.0,0.0,60.0,83.0,Infinity
+0.0,-0.0,0.0,59.0,83.0,Infinity
+0.0,-0.0,0.0,58.0,84.0,Infinity
+0.0,-0.0,0.0,57.0,84.0,Infinity
+0.0,-0.0,0.0,56.0,84.0,Infinity
+0.0,-0.0,0.0,55.0,84.0,Infinity
+0.0,-0.0,0.0,54.0,86.0,Infinity
+0.0,-0.0,0.0,53.0,94.0,Infinity
+0.0,-0.0,0.0,52.0,37.0,Infinity
+0.0,-0.0,0.0,51.0,36.0,Infinity
+0.0,-0.0,0.0,50.0,36.0,Infinity
+0.0,-0.0,0.0,49.0,34.0,Infinity
+0.0,-0.0,0.0,48.0,33.0,Infinity
+0.0,-0.0,0.0,47.0,33.0,Infinity
+0.0,-0.0,0.0,46.0,33.0,Infinity
+0.0,-0.0,0.0,45.0,32.0,Infinity
+0.0,-0.0,0.0,44.0,34.0,Infinity
+0.0,-0.0,0.0,43.0,33.0,Infinity
+0.0,-0.0,0.0,42.0,98.0,Infinity
+0.0,-0.0,0.0,41.0,99.0,Infinity
+0.0,-0.0,0.0,40.0,31.0,Infinity
+0.0,-0.0,0.0,39.0,31.0,Infinity
+0.0,-0.0,0.0,38.0,30.0,Infinity
+0.0,-0.0,0.0,37.0,30.0,Infinity
+0.0,-0.0,0.0,36.0,29.0,Infinity
+0.0,-0.0,0.0,35.0,29.0,Infinity
+0.0,-0.0,0.0,34.0,29.0,Infinity
+0.0,-0.0,0.0,33.0,29.0,Infinity
+0.0,-0.0,0.0,32.0,28.0,Infinity
+0.0,-0.0,0.0,31.0,28.0,Infinity
+0.0,-0.0,0.0,30.0,28.0,Infinity
+0.0,-0.0,0.0,29.0,28.0,Infinity
+0.0,-0.0,0.0,28.0,28.0,Infinity
+0.0,-0.0,0.0,27.0,29.0,Infinity
+0.0,-0.0,0.0,26.0,29.0,Infinity
+0.0,-0.0,0.0,25.0,29.0,Infinity
+0.0,-0.0,0.0,24.0,29.0,Infinity
+0.0,-0.0,0.0,23.0,29.0,Infinity
+0.0,-0.0,0.0,22.0,29.0,Infinity
+0.0,-0.0,0.0,21.0,29.0,Infinity
+0.0,-0.0,0.0,20.0,28.0,Infinity
+0.0,-0.0,0.0,19.0,27.0,Infinity
+0.0,-0.0,0.0,18.0,27.0,Infinity
+0.0,-0.0,0.0,17.0,27.0,Infinity
+0.0,-0.0,0.0,16.0,27.0,Infinity
+0.0,-0.0,0.0,15.0,27.0,Infinity
+0.0,-0.0,0.0,14.0,26.0,Infinity
+0.0,-0.0,0.0,13.0,26.0,Infinity
+0.0,-0.0,0.0,12.0,26.0,Infinity
+0.0,-0.0,0.0,11.0,26.0,Infinity
+0.0,-0.0,0.0,10.0,26.0,Infinity
+0.0,-0.0,0.0,9.0,26.0,Infinity
+0.0,-0.0,0.0,8.0,26.0,Infinity
+0.0,-0.0,0.0,7.0,27.0,Infinity
+0.0,-0.0,0.0,6.0,27.0,Infinity
+0.0,-0.0,0.0,5.0,27.0,Infinity
+0.0,-0.0,0.0,4.0,27.0,Infinity
+0.0,-0.0,0.0,3.0,27.0,Infinity
+0.0,-0.0,0.0,2.0,27.0,Infinity
+0.0,-0.0,0.0,1.0,27.0,59.38700485229492
+0.0,-0.0,0.0,0.0,27.0,57.61711502075195
+0.0,-0.0,0.0,-1.0,27.0,55.96622085571289
+0.0,-0.0,0.0,-2.0,27.0,54.42341613769531
+0.0,-0.0,0.0,-3.0,27.0,52.979087829589844
+0.0,-0.0,0.0,-4.0,27.0,51.6247673034668
+0.0,-0.0,0.0,-5.0,27.0,50.35291290283203
+0.0,-0.0,0.0,-6.0,27.0,49.15683364868164
+0.0,-0.0,0.0,-7.0,27.0,48.030548095703125
+0.0,-0.0,0.0,-8.0,27.0,46.968711853027344
+0.0,-0.0,0.0,-9.0,27.0,45.96649932861328
+0.0,-0.0,0.0,-10.0,27.0,45.019596099853516
+0.0,-0.0,0.0,-11.0,27.0,44.124088287353516
+0.0,-0.0,0.0,-12.0,27.0,43.276432037353516
+0.0,-0.0,0.0,-13.0,27.0,42.47342300415039
+0.0,-0.0,0.0,-14.0,27.0,41.71215057373047
+0.0,-0.0,0.0,-15.0,27.0,40.989952087402344
+0.0,-0.0,0.0,-16.0,27.0,40.30440139770508
+0.0,-0.0,0.0,-17.0,27.0,39.653289794921875
+0.0,-0.0,0.0,-18.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-19.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-20.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-21.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-22.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-23.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-24.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-25.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-26.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-27.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-28.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-29.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-30.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-31.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-32.0,27.0,39.37942123413086
+0.0,-0.0,0.0,-33.0,28.0,40.093597412109375
+0.0,-0.0,0.0,-34.0,28.0,40.84682083129883
+0.0,-0.0,0.0,-35.0,28.0,41.641815185546875
+0.0,-0.0,0.0,-36.0,28.0,42.48157501220703
+0.0,-0.0,0.0,-37.0,28.0,43.36937713623047
+0.0,-0.0,0.0,-38.0,28.0,44.30885696411133
+0.0,-0.0,0.0,-39.0,28.0,45.30405807495117
+0.0,-0.0,0.0,-40.0,28.0,46.35943603515625
+0.0,-0.0,0.0,-41.0,28.0,47.47996139526367
+0.0,-0.0,0.0,-42.0,28.0,48.67118835449219
+0.0,-0.0,0.0,-43.0,28.0,47.415889739990234
+0.0,-0.0,0.0,-44.0,29.0,43.62605667114258
+0.0,-0.0,0.0,-45.0,29.0,42.71345520019531
+0.0,-0.0,0.0,-46.0,29.0,42.71345520019531
+0.0,-0.0,0.0,-47.0,29.0,42.71345520019531
+0.0,-0.0,0.0,-48.0,29.0,42.71345520019531
+0.0,-0.0,0.0,-49.0,30.0,42.71345520019531
+0.0,-0.0,0.0,-50.0,31.0,42.71345520019531
+0.0,-0.0,0.0,-51.0,30.0,42.71345520019531
+0.0,-0.0,0.0,-52.0,31.0,42.71345520019531
+0.0,-0.0,0.0,-53.0,31.0,42.71345520019531
+0.0,-0.0,0.0,-54.0,30.0,42.71345520019531
+0.0,-0.0,0.0,-55.0,30.0,42.71345520019531
+0.0,-0.0,0.0,-56.0,31.0,42.71345520019531
+0.0,-0.0,0.0,-57.0,31.0,42.71345520019531
+0.0,-0.0,0.0,-58.0,53.0,42.71345520019531
+0.0,-0.0,0.0,-59.0,41.0,42.71345520019531
+0.0,-0.0,0.0,-60.0,42.0,42.76785659790039
+0.0,-0.0,0.0,-61.0,42.0,42.83545684814453
+0.0,-0.0,0.0,-62.0,42.0,42.91636657714844
+0.0,-0.0,0.0,-63.0,43.0,43.01070785522461
+0.0,-0.0,0.0,-64.0,42.0,43.11863327026367
+0.0,-0.0,0.0,-65.0,43.0,43.240299224853516
+0.0,-0.0,0.0,-66.0,42.0,43.375911712646484
+0.0,-0.0,0.0,-67.0,42.0,43.52566909790039
+0.0,-0.0,0.0,-68.0,44.0,43.6898193359375
+0.0,-0.0,0.0,-69.0,44.0,43.86863327026367
+0.0,-0.0,0.0,-70.0,45.0,44.062381744384766
+0.0,-0.0,0.0,-71.0,45.0,44.27140808105469
+0.0,-0.0,0.0,-72.0,46.0,44.49604034423828
+0.0,-0.0,0.0,-73.0,46.0,44.73666000366211
+0.0,-0.0,0.0,-74.0,45.0,44.993675231933594
+0.0,-0.0,0.0,-75.0,44.0,45.26752853393555
+0.0,-0.0,0.0,-76.0,42.0,45.55869674682617
+0.0,-0.0,0.0,-77.0,40.0,45.86770248413086
+0.0,-0.0,0.0,-78.0,39.0,46.195091247558594
+0.0,-0.0,0.0,-79.0,40.0,46.54146957397461
+0.0,-0.0,0.0,-80.0,40.0,46.90747833251953
+0.0,-0.0,0.0,-81.0,41.0,47.29380798339844
+0.0,-0.0,0.0,-82.0,41.0,47.701210021972656
+0.0,-0.0,0.0,-83.0,41.0,48.130470275878906
+0.0,-0.0,0.0,-84.0,42.0,48.58246994018555
+0.0,-0.0,0.0,-85.0,42.0,49.0581169128418
+0.0,-0.0,0.0,-86.0,42.0,49.558414459228516
+0.0,-0.0,0.0,-87.0,42.0,50.084442138671875
+0.0,-0.0,0.0,-88.0,43.0,50.6373405456543
+0.0,-0.0,0.0,-89.0,43.0,Infinity

Sonar/692264.txt

@@ -1,181 +0,0 @@
-x,y,headinng,sonar_ang,read,expected
-0.0,-0.0,0.0,0.0,29.0,57.61711502075195
-0.0,-0.0,0.0,1.0,29.0,59.38700485229492
-0.0,-0.0,0.0,2.0,29.0,Infinity
-0.0,-0.0,0.0,3.0,29.0,Infinity
-0.0,-0.0,0.0,4.0,29.0,Infinity
-0.0,-0.0,0.0,5.0,29.0,Infinity
-0.0,-0.0,0.0,6.0,30.0,Infinity
-0.0,-0.0,0.0,7.0,30.0,Infinity
-0.0,-0.0,0.0,8.0,30.0,Infinity
-0.0,-0.0,0.0,9.0,30.0,Infinity
-0.0,-0.0,0.0,10.0,29.0,Infinity
-0.0,-0.0,0.0,11.0,29.0,Infinity
-0.0,-0.0,0.0,12.0,29.0,Infinity
-0.0,-0.0,0.0,13.0,29.0,Infinity
-0.0,-0.0,0.0,14.0,29.0,Infinity
-0.0,-0.0,0.0,15.0,29.0,Infinity
-0.0,-0.0,0.0,16.0,29.0,Infinity
-0.0,-0.0,0.0,17.0,29.0,Infinity
-0.0,-0.0,0.0,18.0,29.0,Infinity
-0.0,-0.0,0.0,19.0,29.0,Infinity
-0.0,-0.0,0.0,20.0,29.0,Infinity
-0.0,-0.0,0.0,21.0,29.0,Infinity
-0.0,-0.0,0.0,22.0,29.0,Infinity
-0.0,-0.0,0.0,23.0,29.0,Infinity
-0.0,-0.0,0.0,24.0,30.0,Infinity
-0.0,-0.0,0.0,25.0,30.0,Infinity
-0.0,-0.0,0.0,26.0,30.0,Infinity
-0.0,-0.0,0.0,27.0,30.0,Infinity
-0.0,-0.0,0.0,28.0,30.0,Infinity
-0.0,-0.0,0.0,29.0,30.0,Infinity
-0.0,-0.0,0.0,30.0,31.0,Infinity
-0.0,-0.0,0.0,31.0,31.0,Infinity
-0.0,-0.0,0.0,32.0,31.0,Infinity
-0.0,-0.0,0.0,33.0,31.0,Infinity
-0.0,-0.0,0.0,34.0,32.0,Infinity
-0.0,-0.0,0.0,35.0,32.0,Infinity
-0.0,-0.0,0.0,36.0,32.0,Infinity
-0.0,-0.0,0.0,37.0,32.0,Infinity
-0.0,-0.0,0.0,38.0,32.0,Infinity
-0.0,-0.0,0.0,39.0,32.0,Infinity
-0.0,-0.0,0.0,40.0,31.0,Infinity
-0.0,-0.0,0.0,41.0,31.0,Infinity
-0.0,-0.0,0.0,42.0,31.0,Infinity
-0.0,-0.0,0.0,43.0,31.0,Infinity
-0.0,-0.0,0.0,44.0,30.0,Infinity
-0.0,-0.0,0.0,45.0,30.0,Infinity
-0.0,-0.0,0.0,46.0,30.0,Infinity
-0.0,-0.0,0.0,47.0,30.0,Infinity
-0.0,-0.0,0.0,48.0,30.0,Infinity
-0.0,-0.0,0.0,49.0,30.0,Infinity
-0.0,-0.0,0.0,50.0,31.0,Infinity
-0.0,-0.0,0.0,51.0,32.0,Infinity
-0.0,-0.0,0.0,52.0,33.0,Infinity
-0.0,-0.0,0.0,53.0,34.0,Infinity
-0.0,-0.0,0.0,54.0,33.0,Infinity
-0.0,-0.0,0.0,55.0,33.0,Infinity
-0.0,-0.0,0.0,56.0,34.0,Infinity
-0.0,-0.0,0.0,57.0,34.0,Infinity
-0.0,-0.0,0.0,58.0,34.0,Infinity
-0.0,-0.0,0.0,59.0,34.0,Infinity
-0.0,-0.0,0.0,60.0,38.0,Infinity
-0.0,-0.0,0.0,61.0,39.0,Infinity
-0.0,-0.0,0.0,62.0,85.0,Infinity
-0.0,-0.0,0.0,63.0,85.0,Infinity
-0.0,-0.0,0.0,64.0,86.0,Infinity
-0.0,-0.0,0.0,65.0,86.0,Infinity
-0.0,-0.0,0.0,66.0,86.0,Infinity
-0.0,-0.0,0.0,67.0,87.0,Infinity
-0.0,-0.0,0.0,68.0,88.0,Infinity
-0.0,-0.0,0.0,69.0,87.0,Infinity
-0.0,-0.0,0.0,70.0,89.0,Infinity
-0.0,-0.0,0.0,71.0,87.0,Infinity
-0.0,-0.0,0.0,72.0,91.0,Infinity
-0.0,-0.0,0.0,75.0,39.0,Infinity
-0.0,-0.0,0.0,76.0,84.0,Infinity
-0.0,-0.0,0.0,77.0,84.0,Infinity
-0.0,-0.0,0.0,78.0,84.0,Infinity
-0.0,-0.0,0.0,79.0,84.0,Infinity
-0.0,-0.0,0.0,80.0,84.0,Infinity
-0.0,-0.0,0.0,81.0,84.0,Infinity
-0.0,-0.0,0.0,82.0,85.0,Infinity
-0.0,-0.0,0.0,83.0,85.0,Infinity
-0.0,-0.0,0.0,84.0,88.0,Infinity
-0.0,-0.0,0.0,85.0,88.0,Infinity
-0.0,-0.0,0.0,86.0,88.0,Infinity
-0.0,-0.0,0.0,87.0,89.0,Infinity
-0.0,-0.0,0.0,88.0,89.0,Infinity
-0.0,-0.0,0.0,89.0,90.0,Infinity
-0.0,-0.0,0.0,90.0,90.0,Infinity
-0.0,-0.0,0.0,89.0,91.0,Infinity
-0.0,-0.0,0.0,88.0,90.0,Infinity
-0.0,-0.0,0.0,87.0,90.0,Infinity
-0.0,-0.0,0.0,86.0,89.0,Infinity
-0.0,-0.0,0.0,85.0,88.0,Infinity
-0.0,-0.0,0.0,84.0,88.0,Infinity
-0.0,-0.0,0.0,83.0,87.0,Infinity
-0.0,-0.0,0.0,82.0,87.0,Infinity
-0.0,-0.0,0.0,81.0,84.0,Infinity
-0.0,-0.0,0.0,80.0,84.0,Infinity
-0.0,-0.0,0.0,79.0,84.0,Infinity
-0.0,-0.0,0.0,78.0,84.0,Infinity
-0.0,-0.0,0.0,77.0,84.0,Infinity
-0.0,-0.0,0.0,76.0,84.0,Infinity
-0.0,-0.0,0.0,75.0,38.0,Infinity
-0.0,-0.0,0.0,74.0,39.0,Infinity
-0.0,-0.0,0.0,73.0,39.0,Infinity
-0.0,-0.0,0.0,72.0,87.0,Infinity
-0.0,-0.0,0.0,71.0,89.0,Infinity
-0.0,-0.0,0.0,70.0,88.0,Infinity
-0.0,-0.0,0.0,69.0,87.0,Infinity
-0.0,-0.0,0.0,68.0,86.0,Infinity
-0.0,-0.0,0.0,67.0,86.0,Infinity
-0.0,-0.0,0.0,66.0,86.0,Infinity
-0.0,-0.0,0.0,65.0,85.0,Infinity
-0.0,-0.0,0.0,64.0,84.0,Infinity
-0.0,-0.0,0.0,63.0,85.0,Infinity
-0.0,-0.0,0.0,62.0,84.0,Infinity
-0.0,-0.0,0.0,61.0,84.0,Infinity
-0.0,-0.0,0.0,60.0,85.0,Infinity
-0.0,-0.0,0.0,59.0,85.0,Infinity
-0.0,-0.0,0.0,58.0,34.0,Infinity
-0.0,-0.0,0.0,57.0,34.0,Infinity
-0.0,-0.0,0.0,56.0,33.0,Infinity
-0.0,-0.0,0.0,55.0,33.0,Infinity
-0.0,-0.0,0.0,54.0,34.0,Infinity
-0.0,-0.0,0.0,53.0,34.0,Infinity
-0.0,-0.0,0.0,52.0,34.0,Infinity
-0.0,-0.0,0.0,51.0,34.0,Infinity
-0.0,-0.0,0.0,50.0,34.0,Infinity
-0.0,-0.0,0.0,49.0,34.0,Infinity
-0.0,-0.0,0.0,48.0,33.0,Infinity
-0.0,-0.0,0.0,47.0,32.0,Infinity
-0.0,-0.0,0.0,46.0,32.0,Infinity
-0.0,-0.0,0.0,45.0,31.0,Infinity
-0.0,-0.0,0.0,44.0,30.0,Infinity
-0.0,-0.0,0.0,43.0,30.0,Infinity
-0.0,-0.0,0.0,42.0,30.0,Infinity
-0.0,-0.0,0.0,41.0,30.0,Infinity
-0.0,-0.0,0.0,40.0,30.0,Infinity
-0.0,-0.0,0.0,39.0,31.0,Infinity
-0.0,-0.0,0.0,38.0,31.0,Infinity
-0.0,-0.0,0.0,37.0,32.0,Infinity
-0.0,-0.0,0.0,36.0,32.0,Infinity
-0.0,-0.0,0.0,35.0,32.0,Infinity
-0.0,-0.0,0.0,34.0,32.0,Infinity
-0.0,-0.0,0.0,33.0,32.0,Infinity
-0.0,-0.0,0.0,32.0,32.0,Infinity
-0.0,-0.0,0.0,31.0,32.0,Infinity
-0.0,-0.0,0.0,30.0,32.0,Infinity
-0.0,-0.0,0.0,29.0,32.0,Infinity
-0.0,-0.0,0.0,28.0,31.0,Infinity
-0.0,-0.0,0.0,27.0,31.0,Infinity
-0.0,-0.0,0.0,26.0,30.0,Infinity
-0.0,-0.0,0.0,25.0,30.0,Infinity
-0.0,-0.0,0.0,24.0,30.0,Infinity
-0.0,-0.0,0.0,23.0,30.0,Infinity
-0.0,-0.0,0.0,22.0,30.0,Infinity
-0.0,-0.0,0.0,21.0,30.0,Infinity
-0.0,-0.0,0.0,20.0,29.0,Infinity
-0.0,-0.0,0.0,19.0,29.0,Infinity
-0.0,-0.0,0.0,18.0,29.0,Infinity
-0.0,-0.0,0.0,17.0,29.0,Infinity
-0.0,-0.0,0.0,16.0,29.0,Infinity
-0.0,-0.0,0.0,15.0,29.0,Infinity
-0.0,-0.0,0.0,14.0,29.0,Infinity
-0.0,-0.0,0.0,13.0,29.0,Infinity
-0.0,-0.0,0.0,12.0,29.0,Infinity
-0.0,-0.0,0.0,11.0,29.0,Infinity
-0.0,-0.0,0.0,10.0,29.0,Infinity
-0.0,-0.0,0.0,9.0,29.0,Infinity
-0.0,-0.0,0.0,8.0,30.0,Infinity
-0.0,-0.0,0.0,7.0,30.0,Infinity
-0.0,-0.0,0.0,6.0,30.0,Infinity
-0.0,-0.0,0.0,5.0,30.0,Infinity
-0.0,-0.0,0.0,4.0,30.0,Infinity
-0.0,-0.0,0.0,3.0,30.0,Infinity
-0.0,-0.0,0.0,2.0,30.0,Infinity
-0.0,-0.0,0.0,1.0,29.0,59.38700485229492
-0.0,-0.0,0.0,0.0,29.0,57.61711502075195
-0.0,-0.0,0.0,-1.0,29.0,55.96622085571289

Sonar/701668.txt

@@ -0,0 +1,181 @@
+x,y,headinng,sonar_ang,read,expected
+0.0,-0.0,0.0,90.0,42.0,Infinity
+0.0,-0.0,0.0,89.0,28.0,Infinity
+0.0,-0.0,0.0,88.0,27.0,Infinity
+0.0,-0.0,0.0,87.0,26.0,Infinity
+0.0,-0.0,0.0,86.0,26.0,Infinity
+0.0,-0.0,0.0,85.0,26.0,Infinity
+0.0,-0.0,0.0,84.0,26.0,Infinity
+0.0,-0.0,0.0,83.0,26.0,Infinity
+0.0,-0.0,0.0,82.0,27.0,Infinity
+0.0,-0.0,0.0,81.0,27.0,Infinity
+0.0,-0.0,0.0,80.0,44.0,Infinity
+0.0,-0.0,0.0,79.0,83.0,Infinity
+0.0,-0.0,0.0,78.0,83.0,Infinity
+0.0,-0.0,0.0,77.0,27.0,Infinity
+0.0,-0.0,0.0,76.0,27.0,Infinity
+0.0,-0.0,0.0,75.0,26.0,Infinity
+0.0,-0.0,0.0,74.0,26.0,Infinity
+0.0,-0.0,0.0,73.0,26.0,Infinity
+0.0,-0.0,0.0,72.0,26.0,Infinity
+0.0,-0.0,0.0,71.0,26.0,Infinity
+0.0,-0.0,0.0,70.0,26.0,Infinity
+0.0,-0.0,0.0,69.0,26.0,Infinity
+0.0,-0.0,0.0,68.0,26.0,Infinity
+0.0,-0.0,0.0,67.0,26.0,Infinity
+0.0,-0.0,0.0,66.0,26.0,Infinity
+0.0,-0.0,0.0,65.0,26.0,Infinity
+0.0,-0.0,0.0,64.0,26.0,Infinity
+0.0,-0.0,0.0,63.0,26.0,Infinity
+0.0,-0.0,0.0,62.0,26.0,Infinity
+0.0,-0.0,0.0,61.0,26.0,Infinity
+0.0,-0.0,0.0,60.0,26.0,Infinity
+0.0,-0.0,0.0,59.0,27.0,Infinity
+0.0,-0.0,0.0,58.0,28.0,Infinity
+0.0,-0.0,0.0,57.0,27.0,Infinity
+0.0,-0.0,0.0,56.0,88.0,Infinity
+0.0,-0.0,0.0,55.0,86.0,Infinity
+0.0,-0.0,0.0,54.0,87.0,Infinity
+0.0,-0.0,0.0,53.0,87.0,Infinity
+0.0,-0.0,0.0,52.0,-1.0,Infinity
+0.0,-0.0,0.0,51.0,27.0,Infinity
+0.0,-0.0,0.0,50.0,27.0,Infinity
+0.0,-0.0,0.0,49.0,27.0,Infinity
+0.0,-0.0,0.0,48.0,27.0,Infinity
+0.0,-0.0,0.0,47.0,27.0,Infinity
+0.0,-0.0,0.0,46.0,27.0,Infinity
+0.0,-0.0,0.0,45.0,28.0,Infinity
+0.0,-0.0,0.0,44.0,28.0,Infinity
+0.0,-0.0,0.0,43.0,28.0,Infinity
+0.0,-0.0,0.0,42.0,45.0,Infinity
+0.0,-0.0,0.0,41.0,45.0,Infinity
+0.0,-0.0,0.0,40.0,45.0,Infinity
+0.0,-0.0,0.0,39.0,45.0,Infinity
+0.0,-0.0,0.0,38.0,44.0,Infinity
+0.0,-0.0,0.0,37.0,44.0,Infinity
+0.0,-0.0,0.0,36.0,43.0,Infinity
+0.0,-0.0,0.0,35.0,43.0,Infinity
+0.0,-0.0,0.0,34.0,44.0,Infinity
+0.0,-0.0,0.0,33.0,44.0,Infinity
+0.0,-0.0,0.0,32.0,45.0,Infinity
+0.0,-0.0,0.0,31.0,44.0,Infinity
+0.0,-0.0,0.0,30.0,44.0,Infinity
+0.0,-0.0,0.0,29.0,44.0,Infinity
+0.0,-0.0,0.0,28.0,44.0,Infinity
+0.0,-0.0,0.0,27.0,44.0,Infinity
+0.0,-0.0,0.0,26.0,44.0,Infinity
+0.0,-0.0,0.0,25.0,44.0,Infinity
+0.0,-0.0,0.0,24.0,44.0,Infinity
+0.0,-0.0,0.0,23.0,43.0,Infinity
+0.0,-0.0,0.0,22.0,43.0,Infinity
+0.0,-0.0,0.0,21.0,43.0,Infinity
+0.0,-0.0,0.0,20.0,43.0,Infinity
+0.0,-0.0,0.0,19.0,42.0,Infinity
+0.0,-0.0,0.0,18.0,42.0,Infinity
+0.0,-0.0,0.0,17.0,42.0,Infinity
+0.0,-0.0,0.0,16.0,39.0,Infinity
+0.0,-0.0,0.0,15.0,38.0,Infinity
+0.0,-0.0,0.0,14.0,38.0,Infinity
+0.0,-0.0,0.0,13.0,38.0,Infinity
+0.0,-0.0,0.0,12.0,40.0,Infinity
+0.0,-0.0,0.0,11.0,40.0,Infinity
+0.0,-0.0,0.0,10.0,43.0,Infinity
+0.0,-0.0,0.0,9.0,43.0,Infinity
+0.0,-0.0,0.0,8.0,43.0,Infinity
+0.0,-0.0,0.0,7.0,43.0,Infinity
+0.0,-0.0,0.0,6.0,43.0,Infinity
+0.0,-0.0,0.0,5.0,43.0,Infinity
+0.0,-0.0,0.0,4.0,43.0,Infinity
+0.0,-0.0,0.0,3.0,43.0,Infinity
+0.0,-0.0,0.0,2.0,42.0,Infinity
+0.0,-0.0,0.0,1.0,42.0,59.38700485229492
+0.0,-0.0,0.0,0.0,42.0,57.61711502075195
+0.0,-0.0,0.0,-1.0,42.0,55.96622085571289
+0.0,-0.0,0.0,-2.0,42.0,54.42341613769531
+0.0,-0.0,0.0,-3.0,42.0,52.979087829589844
+0.0,-0.0,0.0,-4.0,42.0,51.6247673034668
+0.0,-0.0,0.0,-5.0,42.0,50.35291290283203
+0.0,-0.0,0.0,-6.0,41.0,49.15683364868164
+0.0,-0.0,0.0,-7.0,41.0,48.030548095703125
+0.0,-0.0,0.0,-8.0,39.0,46.968711853027344
+0.0,-0.0,0.0,-9.0,39.0,45.96649932861328
+0.0,-0.0,0.0,-10.0,39.0,45.019596099853516
+0.0,-0.0,0.0,-11.0,36.0,44.124088287353516
+0.0,-0.0,0.0,-12.0,36.0,43.276432037353516
+0.0,-0.0,0.0,-13.0,36.0,42.47342300415039
+0.0,-0.0,0.0,-14.0,35.0,41.71215057373047
+0.0,-0.0,0.0,-15.0,35.0,40.989952087402344
+0.0,-0.0,0.0,-16.0,35.0,40.30440139770508
+0.0,-0.0,0.0,-17.0,35.0,39.653289794921875
+0.0,-0.0,0.0,-18.0,36.0,39.37942123413086
+0.0,-0.0,0.0,-19.0,42.0,39.37942123413086
+0.0,-0.0,0.0,-20.0,42.0,39.37942123413086
+0.0,-0.0,0.0,-21.0,42.0,39.37942123413086
+0.0,-0.0,0.0,-22.0,41.0,39.37942123413086
+0.0,-0.0,0.0,-23.0,41.0,39.37942123413086
+0.0,-0.0,0.0,-24.0,40.0,39.37942123413086
+0.0,-0.0,0.0,-25.0,37.0,39.37942123413086
+0.0,-0.0,0.0,-26.0,36.0,39.37942123413086
+0.0,-0.0,0.0,-27.0,36.0,39.37942123413086
+0.0,-0.0,0.0,-28.0,35.0,39.37942123413086
+0.0,-0.0,0.0,-29.0,35.0,39.37942123413086
+0.0,-0.0,0.0,-30.0,35.0,39.37942123413086
+0.0,-0.0,0.0,-31.0,34.0,39.37942123413086
+0.0,-0.0,0.0,-32.0,34.0,39.37942123413086
+0.0,-0.0,0.0,-33.0,35.0,40.093597412109375
+0.0,-0.0,0.0,-34.0,36.0,40.84682083129883
+0.0,-0.0,0.0,-35.0,36.0,41.641815185546875
+0.0,-0.0,0.0,-36.0,37.0,42.48157501220703
+0.0,-0.0,0.0,-37.0,39.0,43.36937713623047
+0.0,-0.0,0.0,-38.0,40.0,44.30885696411133
+0.0,-0.0,0.0,-39.0,40.0,45.30405807495117
+0.0,-0.0,0.0,-40.0,42.0,46.35943603515625
+0.0,-0.0,0.0,-41.0,43.0,47.47996139526367
+0.0,-0.0,0.0,-42.0,42.0,48.67118835449219
+0.0,-0.0,0.0,-43.0,44.0,47.415889739990234
+0.0,-0.0,0.0,-44.0,44.0,43.62605667114258
+0.0,-0.0,0.0,-45.0,46.0,42.71345520019531
+0.0,-0.0,0.0,-46.0,46.0,42.71345520019531
+0.0,-0.0,0.0,-47.0,46.0,42.71345520019531
+0.0,-0.0,0.0,-48.0,46.0,42.71345520019531
+0.0,-0.0,0.0,-49.0,46.0,42.71345520019531
+0.0,-0.0,0.0,-50.0,57.0,42.71345520019531
+0.0,-0.0,0.0,-51.0,57.0,42.71345520019531
+0.0,-0.0,0.0,-52.0,57.0,42.71345520019531
+0.0,-0.0,0.0,-53.0,61.0,42.71345520019531
+0.0,-0.0,0.0,-54.0,62.0,42.71345520019531
+0.0,-0.0,0.0,-55.0,62.0,42.71345520019531
+0.0,-0.0,0.0,-56.0,62.0,42.71345520019531
+0.0,-0.0,0.0,-57.0,63.0,42.71345520019531
+0.0,-0.0,0.0,-58.0,65.0,42.71345520019531
+0.0,-0.0,0.0,-59.0,-1.0,42.71345520019531
+0.0,-0.0,0.0,-60.0,-1.0,42.76785659790039
+0.0,-0.0,0.0,-61.0,-1.0,42.83545684814453
+0.0,-0.0,0.0,-62.0,-1.0,42.91636657714844
+0.0,-0.0,0.0,-63.0,59.0,43.01070785522461
+0.0,-0.0,0.0,-64.0,59.0,43.11863327026367
+0.0,-0.0,0.0,-65.0,-1.0,43.240299224853516
+0.0,-0.0,0.0,-66.0,59.0,43.375911712646484
+0.0,-0.0,0.0,-67.0,-1.0,43.52566909790039
+0.0,-0.0,0.0,-68.0,-1.0,43.6898193359375
+0.0,-0.0,0.0,-69.0,-117.0,43.86863327026367
+0.0,-0.0,0.0,-70.0,100.0,44.062381744384766
+0.0,-0.0,0.0,-71.0,99.0,44.27140808105469
+0.0,-0.0,0.0,-72.0,99.0,44.49604034423828
+0.0,-0.0,0.0,-73.0,99.0,44.73666000366211
+0.0,-0.0,0.0,-74.0,99.0,44.993675231933594
+0.0,-0.0,0.0,-75.0,99.0,45.26752853393555
+0.0,-0.0,0.0,-76.0,100.0,45.55869674682617
+0.0,-0.0,0.0,-77.0,99.0,45.86770248413086
+0.0,-0.0,0.0,-78.0,-120.0,46.195091247558594
+0.0,-0.0,0.0,-79.0,-120.0,46.54146957397461
+0.0,-0.0,0.0,-80.0,-120.0,46.90747833251953
+0.0,-0.0,0.0,-81.0,-120.0,47.29380798339844
+0.0,-0.0,0.0,-82.0,-120.0,47.701210021972656
+0.0,-0.0,0.0,-83.0,-120.0,48.130470275878906
+0.0,-0.0,0.0,-84.0,-121.0,48.58246994018555
+0.0,-0.0,0.0,-85.0,-121.0,49.0581169128418
+0.0,-0.0,0.0,-86.0,-121.0,49.558414459228516
+0.0,-0.0,0.0,-87.0,-121.0,50.084442138671875
+0.0,-0.0,0.0,-88.0,-121.0,50.6373405456543
+0.0,-0.0,0.0,-89.0,-121.0,Infinity

Sonar/8601.txt

@@ -1,19 +0,0 @@
-x,y,headinng,sonar_ang,read,expected
-0.0,-0.0,0.0,0.0,35.0,57.61711502075195
-0.0,-0.0,0.0,10.0,37.0,Infinity
-0.0,-0.0,0.0,20.0,44.0,Infinity
-0.0,-0.0,0.0,30.0,44.0,Infinity
-0.0,-0.0,0.0,40.0,46.0,Infinity
-0.0,-0.0,0.0,60.0,27.0,Infinity
-0.0,-0.0,0.0,70.0,28.0,Infinity
-0.0,-0.0,0.0,80.0,28.0,Infinity
-0.0,-0.0,0.0,90.0,29.0,Infinity
-0.0,-0.0,0.0,80.0,83.0,Infinity
-0.0,-0.0,0.0,70.0,28.0,Infinity
-0.0,-0.0,0.0,60.0,28.0,Infinity
-0.0,-0.0,0.0,40.0,46.0,Infinity
-0.0,-0.0,0.0,30.0,43.0,Infinity
-0.0,-0.0,0.0,20.0,44.0,Infinity
-0.0,-0.0,0.0,10.0,37.0,Infinity
-0.0,-0.0,0.0,0.0,35.0,57.61711502075195
-0.0,-0.0,0.0,-10.0,40.0,45.019596099853516

Sonar/src/MainProgram.java

@@ -179,6 +179,7 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		} catch (IOException e) {
 			return;
 		}
+
 		PrintWriter printWriter = new PrintWriter(fileWriter);
 		printWriter.print("x,y,headinng,sonar_ang,read,expected\n");
 		for (DataPose d : data) {
@@ -293,8 +294,8 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		Pose p = data.getPose();
 		System.out.println(p);
 		imap.addPoint(p);
+		scanner.setPose(p);
 		if (data.getDistance() == 255) {
-			scanner.setPose(p);
 			return;
 		}
 

Sonar/src/robots/BluetoothRobot.java

@@ -313,6 +313,15 @@ public class BluetoothRobot implements Robot {
 		} catch (InterruptedException e1) {
 			System.out.println("Nao foi possivel finalizar as threads...");
 		}
+
+		if (sendthread == null) return;
+		sendthread.run = false;
+		try {
+			sendthread.join();
+		} catch (InterruptedException e1) {
+			System.out.println("Nao foi possivel finalizar as threads...");
+		}
+		
 		try {
 			nxtComm.close();
 		} catch (IOException e) {

SonarBrick/src/Sonar.java

@@ -57,18 +57,17 @@ class Scanner extends Thread {
 			float y = pose.getY();
 			float alpha = pose.getHeading();
 			
-			if (distance != 255) {
-				try {
-					output.write('@');
-					output.write(position);
-					output.writeFloat(alpha);
-					output.writeFloat(x);
-					output.write(distance);
-					output.writeFloat(y);
-					output.flush();
-				} catch (IOException e) {
-				}
+			try {
+				output.write('@');
+				output.write(position);
+				output.writeFloat(alpha);
+				output.writeFloat(x);
+				output.write(distance);
+				output.writeFloat(y);
+				output.flush();
+			} catch (IOException e) {
 			}
+		
 			
 			position += increment;
 		}
@@ -147,6 +146,7 @@ public class Sonar {
 					break;
 				case STARTSCANN:
 					scan.position = 90;
+					scan.increment = -Math.abs(scan.increment);
 					scan.scann = true;
 					break;
 				case MOVE: