Adicionando LCP

.metadata/.log

@@ -1475,3 +1475,51 @@ java.lang.NullPointerException
 	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.eclipse.m2e.logback.appender 2 0 2017-10-27 11:49:19.548
+!MESSAGE Failed to download jre:jre:ctor:zip:1.0.0
+!STACK 0
+org.eclipse.aether.resolution.ArtifactResolutionException: Could not transfer artifact jre:jre:zip:ctor:1.0.0-20160630.072510-1 from/to models (http://download.eclipse.org/recommenders/models/oxygen/): Host is down
+	at org.eclipse.aether.internal.impl.DefaultArtifactResolver.resolve(DefaultArtifactResolver.java:453)
+	at org.eclipse.aether.internal.impl.DefaultArtifactResolver.resolveArtifacts(DefaultArtifactResolver.java:255)
+	at org.eclipse.aether.internal.impl.DefaultArtifactResolver.resolveArtifact(DefaultArtifactResolver.java:232)
+	at org.eclipse.aether.internal.impl.DefaultRepositorySystem.resolveArtifact(DefaultRepositorySystem.java:303)
+	at org.eclipse.recommenders.models.ModelRepository.resolveInternal(ModelRepository.java:193)
+	at org.eclipse.recommenders.models.ModelRepository.resolve(ModelRepository.java:172)
+	at org.eclipse.recommenders.internal.models.rcp.EclipseModelRepository.resolve(EclipseModelRepository.java:168)
+	at org.eclipse.recommenders.internal.models.rcp.DownloadModelArchiveJob.run(DownloadModelArchiveJob.java:76)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+Caused by: org.eclipse.aether.transfer.ArtifactTransferException: Could not transfer artifact jre:jre:zip:ctor:1.0.0-20160630.072510-1 from/to models (http://download.eclipse.org/recommenders/models/oxygen/): Host is down
+	at org.eclipse.aether.connector.basic.ArtifactTransportListener.transferFailed(ArtifactTransportListener.java:52)
+	at org.eclipse.aether.connector.basic.BasicRepositoryConnector$TaskRunner.run(BasicRepositoryConnector.java:364)
+	at org.eclipse.aether.util.concurrency.RunnableErrorForwarder$1.run(RunnableErrorForwarder.java:76)
+	at org.eclipse.aether.connector.basic.BasicRepositoryConnector$DirectExecutor.execute(BasicRepositoryConnector.java:590)
+	at org.eclipse.aether.connector.basic.BasicRepositoryConnector.get(BasicRepositoryConnector.java:258)
+	at org.eclipse.aether.internal.impl.DefaultArtifactResolver.performDownloads(DefaultArtifactResolver.java:529)
+	at org.eclipse.aether.internal.impl.DefaultArtifactResolver.resolve(DefaultArtifactResolver.java:430)
+	... 8 more
+Caused by: java.net.ConnectException: Host is down
+	at java.net.PlainSocketImpl.socketConnect(Native Method)
+	at java.net.AbstractPlainSocketImpl.doConnect(AbstractPlainSocketImpl.java:345)
+	at java.net.AbstractPlainSocketImpl.connectToAddress(AbstractPlainSocketImpl.java:206)
+	at java.net.AbstractPlainSocketImpl.connect(AbstractPlainSocketImpl.java:188)
+	at java.net.SocksSocketImpl.connect(SocksSocketImpl.java:392)
+	at java.net.Socket.connect(Socket.java:589)
+	at org.apache.http.conn.scheme.PlainSocketFactory.connectSocket(PlainSocketFactory.java:120)
+	at org.apache.http.impl.conn.DefaultClientConnectionOperator.openConnection(DefaultClientConnectionOperator.java:179)
+	at org.apache.http.impl.conn.ManagedClientConnectionImpl.open(ManagedClientConnectionImpl.java:328)
+	at org.apache.http.impl.client.DefaultRequestDirector.tryConnect(DefaultRequestDirector.java:612)
+	at org.apache.http.impl.client.DefaultRequestDirector.execute(DefaultRequestDirector.java:447)
+	at org.apache.http.impl.client.AbstractHttpClient.doExecute(AbstractHttpClient.java:884)
+	at org.apache.http.impl.client.CloseableHttpClient.execute(CloseableHttpClient.java:71)
+	at org.apache.http.impl.client.CloseableHttpClient.execute(CloseableHttpClient.java:55)
+	at org.apache.http.impl.client.DecompressingHttpClient.execute(DecompressingHttpClient.java:164)
+	at org.eclipse.aether.transport.http.HttpTransporter.execute(HttpTransporter.java:296)
+	at org.eclipse.aether.transport.http.HttpTransporter.implGet(HttpTransporter.java:252)
+	at org.eclipse.aether.spi.connector.transport.AbstractTransporter.get(AbstractTransporter.java:68)
+	at org.eclipse.aether.connector.basic.BasicRepositoryConnector$GetTaskRunner.runTask(BasicRepositoryConnector.java:456)
+	at org.eclipse.aether.connector.basic.BasicRepositoryConnector$TaskRunner.run(BasicRepositoryConnector.java:359)
+	... 13 more
+
+!ENTRY org.eclipse.recommenders.models.rcp 1 0 2017-10-27 11:49:19.551
+!MESSAGE jre:jre:ctor:zip:1.0.0 could not be resolved from the model repositories. Are you offline?

.metadata/.plugins/org.eclipse.core.resources/.history/1/902f28dd22bb001711b8bf632416c20d

@@ -0,0 +1,130 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.NXTRegulatedMotor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/11/40056c7c19bb001711b8bf632416c20d

@@ -0,0 +1,149 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			int ang = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+				
+
+				Pose tmppose = new Pose(pose.getX(), pose.getY(), (float) (pose.getHeading() + ang));
+				float dist = map.range(tmppose);
+				if (dist == -1) dist =  255;
+				
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(ang+90);
+
+				rr.robotData(data);
+
+				ang += add;
+				if (ang == -180 || ang == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+			Pose tmppose = new Pose(pose.getX(), pose.getY(), (float) (pose.getHeading() + ang));
+			float dist = map.range(tmppose);
+			if (dist == -1) dist =  255;
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/17/c06e3aa321bb001711b8bf632416c20d

@@ -0,0 +1,338 @@
+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.LCPRobot;
+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);
+		scanner.setPose(p);
+		if (data.getDistance() == 255) {
+			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);
+		//Robot robotlcp = new LCPRobot();
+
+		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/1b/b0be3c7026bb001711b8bf632416c20d

@@ -0,0 +1,162 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 20;
+		for (int angle=-cone/2; angle <= cone/2; angle++) {
+			tmppose.setHeading((float) (pose.getHeading() + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+rand.nextGaussian()*4+Math.random();
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/1c/10536aec23bb001711b8bf632416c20d

@@ -0,0 +1,126 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			scannerm.rotate(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/2a/80ea20eb20bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        double ang = 0;
+        double engine_mult = 1;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(-90*engine_mult); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(engine_mult*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (engine_mult*end); j+=(engine_mult*interval)){ 
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            
+            DataPose data = new DataPose();
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/2e/3044435021bb001711b8bf632416c20d

@@ -0,0 +1,338 @@
+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.LCPRobot;
+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);
+		scanner.setPose(p);
+		if (data.getDistance() == 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { robotlcp, 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/35/00c1392626bb001711b8bf632416c20d

@@ -0,0 +1,162 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 20;
+		for (int angle=-cone/2; angle <= cone/2; angle++) {
+			tmppose.setHeading((float) (pose.getHeading() - angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+rand.nextGaussian()*4+Math.random();
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/3a/509d862427bb001711b8bf632416c20d

@@ -0,0 +1,161 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+
+				data.setDistance((int) dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle + 90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo = -cone / 2; angulo <= cone / 2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist)
+				mindist = dist;
+		}
+		double v = mindist + rand.nextGaussian() * 4 + Math.random() * 2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float) -x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		for (angle = ini-90; angle <= end-90; angle += interval) {
+			DataPose data = new DataPose();
+			double dist = getDistance();
+			data.setDistance((int) dist);
+			data.setPose(pose);
+			data.setSensorAngle(angle+90);
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null)
+			return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/3a/7045f71822bb001711b8bf632416c20d

@@ -0,0 +1,119 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/40/a0df7c2e21bb001711b8bf632416c20d

@@ -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);
+		scanner.setPose(p);
+		if (data.getDistance() == 255) {
+			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/43/70f5ce0724bb001711b8bf632416c20d

@@ -0,0 +1,127 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			System.out.println(j);
+			scannerm.rotate(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/47/20da3f6b1abb001711b8bf632416c20d

@@ -0,0 +1,161 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				if (dist == -1) dist =  255;
+				
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		} 
+		return mindist;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+			Pose tmppose = new Pose(pose.getX(), pose.getY(), (float) (pose.getHeading() + ang));
+			float dist = map.range(tmppose);
+			if (dist == -1) dist =  255;
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/47/8092227222bb001711b8bf632416c20d

@@ -0,0 +1,126 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/4b/b05c461322bb001711b8bf632416c20d

@@ -0,0 +1,122 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		provider = new OdometryPoseProvider(pilot);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/4d/40a85dea25bb001711b8bf632416c20d

@@ -0,0 +1,162 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 20;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+rand.nextGaussian()*4+Math.random();
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/4e/404f5a3424bb001711b8bf632416c20d

@@ -0,0 +1,126 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			System.out.println(j);
+			scannerm.rotateTo(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/4f/20eb459628bb001711b8bf632416c20d

@@ -0,0 +1,341 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		String s = "LCPRobot";
+
+		Object[] possibleValues = { s, 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/55/d0b009a620bb001711b8bf632416c20d

@@ -0,0 +1,123 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        int i=0;
+        double ang = 0;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            DataPose data = new DataPose();
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/5d/00d711ce21bb001711b8bf632416c20d

@@ -0,0 +1,116 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/6/6066237b1ebb001711b8bf632416c20d

@@ -0,0 +1,305 @@
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.Point;
+import java.awt.event.MouseEvent;
+import java.awt.event.MouseListener;
+import java.awt.event.MouseMotionListener;
+import java.awt.event.MouseWheelEvent;
+import java.awt.event.MouseWheelListener;
+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 MapImage extends JPanel implements MouseWheelListener, MouseListener, MouseMotionListener  {
+    private double zoom = 2.0; // pixel per cm
+    private double grid = 10.0; // cm
+    private double centerx = 0.0;
+    private double centery = 0.0; // cm
+    private Point mousePt;
+    private ArrayList<Pose> lista_pontos;
+    private ArrayList<Pose> lista_ultra;
+    private int visual_method = 0;
+    private boolean line = false;
+    
+    private Semaphore semaphore;
+    
+    private LineMap map;
+    
+	public MapImage() {
+		super();
+		semaphore = new Semaphore(1);
+		lista_pontos = new ArrayList<Pose>();
+		lista_ultra = new ArrayList<Pose>();
+		setBackground(Color.BLACK);
+		addMouseWheelListener(this);
+		addMouseListener(this);
+		addMouseMotionListener(this);
+	}
+	
+	public MapImage(LineMap map) {
+		this();
+		this.map = map;
+	}
+	
+    private void drawModel (Graphics g) {
+        int width = (int) (getWidth()+2*centerx);
+        int height = (int) (getHeight()+2*centery);
+        int count = 0;
+        int x_tmp = 0, y_tmp = 0;
+        
+	    	for (Pose p : lista_pontos) {
+			double hading = Math.toRadians(p.getHeading());
+				
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		
+			if (visual_method == 0) {
+				g.setColor(Color.getHSBColor((float) (hading/(2.0*Math.PI)), 1, 1));
+				g.fillOval(
+						x-(int)(zoom/2.0*1.5),
+						y-(int)(zoom/2.0*1.5),
+						(int)(zoom*1.5),
+						(int)(zoom*1.5)
+				);
+			} else if (visual_method == 1) {
+	            g.setColor(Color.WHITE);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+	
+	           g.drawLine(
+	                width/2+(int)(p.getX()*zoom+zoom*Math.sin(hading)),
+	                height/2-(int)(p.getY()*zoom-zoom*Math.cos(hading)),
+	                width/2+(int)(p.getX()*zoom+0.6*zoom*Math.sin(Math.PI/8+hading)),
+	                height/2-(int)(p.getY()*zoom-0.6*zoom*Math.cos(Math.PI/8+hading))
+	            );
+			} else if (visual_method == 2) {
+				g.setColor(Color.WHITE);
+				g.fillOval(
+						x-(int)(zoom/2.0*2),
+						y-(int)(zoom/2.0*2),
+						(int)(zoom*2),
+						(int)(zoom*2)
+				);
+	            g.setColor(Color.BLACK);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+			}
+	
+		    	if (line && count != 0) {
+		    		g.setColor(Color.LIGHT_GRAY);
+		    		g.drawLine(x_tmp, y_tmp, x, y);
+		    	}
+	
+		    	x_tmp = x;
+		    	y_tmp = y;
+		    	count++;
+		}
+	    	
+	    	g.setColor(Color.RED);
+	    	for (Pose p : lista_ultra) {
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		g.fillRect(
+					x-(int)(zoom/2.0*1.0),
+					y-(int)(zoom/2.0*1.0),
+					(int)(zoom*1.0),
+					(int)(zoom*1.0)
+			);
+	    	}
+	    	
+	    	if (map != null) {
+	    		Line[] lines = map.getLines();
+	    		for (int i = 0; i < lines.length; i++) {
+	    			Line l = lines[i];
+	            g.drawLine(
+		                width/2+(int)(l.x1*zoom),
+		                height/2-(int)(l.y1*zoom), 
+		                width/2+(int)(l.x2*zoom),
+		                height/2-(int)(l.y2*zoom)
+		        );
+	    		}
+	    	}
+    }
+    
+    @Override
+    protected void paintComponent(Graphics g) {
+        int width = (int) (getWidth());
+        int height = (int) (getHeight());
+        int width2 = (int) (getWidth()+2*centerx);
+        int height2 = (int) (getHeight()+2*centery);
+        super.paintComponent(g);
+    
+        g.setColor(new Color(20, 20, 20));
+        
+        int initial_x = height2/2;
+        while (initial_x < width) {
+        	initial_x += grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        initial_x = height2/2;
+        while (initial_x > 0) {
+        	initial_x -= grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        int initial_y = width2/2;
+        while (initial_y < width) {
+        	initial_y += grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+        initial_y = width2/2;
+        while (initial_y > 0) {
+        	initial_y -= grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+
+        g.setColor(Color.ORANGE);
+        g.drawLine(width2/2, 0, width2/2, height);
+        g.drawLine(0, height2/2, width, height2/2);
+
+        if (semaphore.tryAcquire()) {
+	        drawModel(g);
+	        semaphore.release();
+		}
+    }
+    
+    /**
+     * Adiciona um ponto ao mapa
+     * @param p ponto
+     */
+    public void addPoint(Pose p) {
+    		if (semaphore.tryAcquire()) {
+    			lista_pontos.clear();
+    			lista_pontos.add(p);
+	        semaphore.release();
+		}
+    		repaint();
+	}
+
+    public void addPoint(float x, float y, float z) {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.add(new Pose(x, y, z));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addRead(float x, float y) {
+		if (semaphore.tryAcquire()) {
+			lista_ultra.add(new Pose(x, y, 0));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addPoint(double x, double y, double z) {
+    		addPoint((float)x, (float)y, (float)z);
+	}
+    
+
+    public void addRead(double x, double y) {
+    		addRead((float)x, (float)y);
+	}
+    
+    
+    public void showLine () {
+    		line = !line;
+    		repaint();
+    }
+
+    public void setVisual (int method) {
+    		visual_method = method;
+    		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.");
+		}
+    }
+    
+	public void clean() {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.clear();
+			lista_ultra.clear();
+	        semaphore.release();
+		}
+		repaint();
+	}
+	@Override
+	public void mouseDragged(MouseEvent e) {
+		centerx += e.getX() - mousePt.x;
+		centery += e.getY() - mousePt.y;
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseMoved(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseClicked(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mousePressed(MouseEvent e) {
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseReleased(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseEntered(MouseEvent e) {
+	}
+	
+	@Override
+	public void mouseExited(MouseEvent e) {	
+	}
+	
+	@Override
+	public void mouseWheelMoved(MouseWheelEvent e) {
+		if(e.getWheelRotation()<0){
+			if (zoom < 15.0)
+				zoom *= 1.1;
+			repaint();
+		}
+		//Zoom out
+		if(e.getWheelRotation()>0){
+			if (zoom > 1.0)
+				zoom /= 1.1;
+			repaint();
+		}
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/68/d062701a20bb001711b8bf632416c20d

@@ -0,0 +1,146 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    static double theta = 0;
+    static double x = 0;
+    static double y = 0;
+
+    @Override
+    public void move(double x) {
+        pilot.travel(x);
+        x += Math.cos(Math.toRadians(theta)) * x;
+        y += Math.sin(Math.toRadians(theta)) * x;
+        // double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+        // double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+        // pose.translate((float) dx, (float) -dy);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+        if ((theta + x) > 360){
+            int add = (theta + x) % 360;
+            theta += add;
+        }
+        else if ((theta + x) < 0){
+            theta = 360 + (theta + x);
+        }
+        else
+            theta += x;
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        Pose p = new Pose((float) x, (float) y, (float) theta);
+        int val = 0;
+        int i=0;
+        double ang = 0;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            DataPose data = new DataPose();
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(p);
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveRight() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveBackward() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stop() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		// TODO Auto-generated method stub
+		
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/69/203992c022bb001711b8bf632416c20d

@@ -0,0 +1,130 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.NXTRegulatedMotor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor bc = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/69/7011036e1dbb001711b8bf632416c20d

@@ -0,0 +1,159 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+Math.random()*10;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/69/b002cabb22bb001711b8bf632416c20d

@@ -0,0 +1,125 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/6d/40e5319c21bb001711b8bf632416c20d

@@ -0,0 +1,121 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		//provider = new OdometryPoseProvider(pilot);
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/71/c0b0c0ed22bb001711b8bf632416c20d

@@ -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();
+			
+			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/74/9073c18b23bb001711b8bf632416c20d

@@ -0,0 +1,126 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			scannerm.rotate(-(j * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/76/0090d7cc26bb001711b8bf632416c20d

@@ -0,0 +1,340 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { /*robotlcp,*/ 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/7c/009d08eb1fbb001711b8bf632416c20d

@@ -0,0 +1,5 @@
+package robots;
+
+public class LCPRobot {
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/80/50046a971ebb001711b8bf632416c20d

@@ -0,0 +1,305 @@
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.Point;
+import java.awt.event.MouseEvent;
+import java.awt.event.MouseListener;
+import java.awt.event.MouseMotionListener;
+import java.awt.event.MouseWheelEvent;
+import java.awt.event.MouseWheelListener;
+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 MapImage extends JPanel implements MouseWheelListener, MouseListener, MouseMotionListener  {
+    private double zoom = 2.0; // pixel per cm
+    private double grid = 10.0; // cm
+    private double centerx = 0.0;
+    private double centery = 0.0; // cm
+    private Point mousePt;
+    private ArrayList<Pose> lista_pontos;
+    private ArrayList<Pose> lista_ultra;
+    private int visual_method = 0;
+    private boolean line = false;
+    
+    private Semaphore semaphore;
+    
+    private LineMap map;
+    
+	public MapImage() {
+		super();
+		semaphore = new Semaphore(1);
+		lista_pontos = new ArrayList<Pose>();
+		lista_ultra = new ArrayList<Pose>();
+		setBackground(Color.BLACK);
+		addMouseWheelListener(this);
+		addMouseListener(this);
+		addMouseMotionListener(this);
+	}
+	
+	public MapImage(LineMap map) {
+		this();
+		this.map = map;
+	}
+	
+    private void drawModel (Graphics g) {
+        int width = (int) (getWidth()+2*centerx);
+        int height = (int) (getHeight()+2*centery);
+        int count = 0;
+        int x_tmp = 0, y_tmp = 0;
+        
+	    	for (Pose p : lista_pontos) {
+			double hading = Math.toRadians(p.getHeading());
+				
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		
+			if (visual_method == 0) {
+				g.setColor(Color.getHSBColor((float) (hading/(2.0*Math.PI)), 1, 1));
+				g.fillOval(
+						x-(int)(zoom/2.0*1.5),
+						y-(int)(zoom/2.0*1.5),
+						(int)(zoom*1.5),
+						(int)(zoom*1.5)
+				);
+			} else if (visual_method == 1) {
+	            g.setColor(Color.WHITE);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+	
+	           g.drawLine(
+	                width/2+(int)(p.getX()*zoom+zoom*Math.sin(hading)),
+	                height/2-(int)(p.getY()*zoom-zoom*Math.cos(hading)),
+	                width/2+(int)(p.getX()*zoom+0.6*zoom*Math.sin(Math.PI/8+hading)),
+	                height/2-(int)(p.getY()*zoom-0.6*zoom*Math.cos(Math.PI/8+hading))
+	            );
+			} else if (visual_method == 2) {
+				g.setColor(Color.WHITE);
+				g.fillOval(
+						x-(int)(zoom/2.0*5),
+						y-(int)(zoom/2.0*5),
+						(int)(zoom*5),
+						(int)(zoom*5)
+				);
+	            g.setColor(Color.BLACK);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+			}
+	
+		    	if (line && count != 0) {
+		    		g.setColor(Color.LIGHT_GRAY);
+		    		g.drawLine(x_tmp, y_tmp, x, y);
+		    	}
+	
+		    	x_tmp = x;
+		    	y_tmp = y;
+		    	count++;
+		}
+	    	
+	    	g.setColor(Color.RED);
+	    	for (Pose p : lista_ultra) {
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		g.fillRect(
+					x-(int)(zoom/2.0*1.0),
+					y-(int)(zoom/2.0*1.0),
+					(int)(zoom*1.0),
+					(int)(zoom*1.0)
+			);
+	    	}
+	    	
+	    	if (map != null) {
+	    		Line[] lines = map.getLines();
+	    		for (int i = 0; i < lines.length; i++) {
+	    			Line l = lines[i];
+	            g.drawLine(
+		                width/2+(int)(l.x1*zoom),
+		                height/2-(int)(l.y1*zoom), 
+		                width/2+(int)(l.x2*zoom),
+		                height/2-(int)(l.y2*zoom)
+		        );
+	    		}
+	    	}
+    }
+    
+    @Override
+    protected void paintComponent(Graphics g) {
+        int width = (int) (getWidth());
+        int height = (int) (getHeight());
+        int width2 = (int) (getWidth()+2*centerx);
+        int height2 = (int) (getHeight()+2*centery);
+        super.paintComponent(g);
+    
+        g.setColor(new Color(20, 20, 20));
+        
+        int initial_x = height2/2;
+        while (initial_x < width) {
+        	initial_x += grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        initial_x = height2/2;
+        while (initial_x > 0) {
+        	initial_x -= grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        int initial_y = width2/2;
+        while (initial_y < width) {
+        	initial_y += grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+        initial_y = width2/2;
+        while (initial_y > 0) {
+        	initial_y -= grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+
+        g.setColor(Color.ORANGE);
+        g.drawLine(width2/2, 0, width2/2, height);
+        g.drawLine(0, height2/2, width, height2/2);
+
+        if (semaphore.tryAcquire()) {
+	        drawModel(g);
+	        semaphore.release();
+		}
+    }
+    
+    /**
+     * Adiciona um ponto ao mapa
+     * @param p ponto
+     */
+    public void addPoint(Pose p) {
+    		if (semaphore.tryAcquire()) {
+    			lista_pontos.clear();
+    			lista_pontos.add(p);
+	        semaphore.release();
+		}
+    		repaint();
+	}
+
+    public void addPoint(float x, float y, float z) {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.add(new Pose(x, y, z));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addRead(float x, float y) {
+		if (semaphore.tryAcquire()) {
+			lista_ultra.add(new Pose(x, y, 0));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addPoint(double x, double y, double z) {
+    		addPoint((float)x, (float)y, (float)z);
+	}
+    
+
+    public void addRead(double x, double y) {
+    		addRead((float)x, (float)y);
+	}
+    
+    
+    public void showLine () {
+    		line = !line;
+    		repaint();
+    }
+
+    public void setVisual (int method) {
+    		visual_method = method;
+    		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.");
+		}
+    }
+    
+	public void clean() {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.clear();
+			lista_ultra.clear();
+	        semaphore.release();
+		}
+		repaint();
+	}
+	@Override
+	public void mouseDragged(MouseEvent e) {
+		centerx += e.getX() - mousePt.x;
+		centery += e.getY() - mousePt.y;
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseMoved(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseClicked(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mousePressed(MouseEvent e) {
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseReleased(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseEntered(MouseEvent e) {
+	}
+	
+	@Override
+	public void mouseExited(MouseEvent e) {	
+	}
+	
+	@Override
+	public void mouseWheelMoved(MouseWheelEvent e) {
+		if(e.getWheelRotation()<0){
+			if (zoom < 15.0)
+				zoom *= 1.1;
+			repaint();
+		}
+		//Zoom out
+		if(e.getWheelRotation()>0){
+			if (zoom > 1.0)
+				zoom /= 1.1;
+			repaint();
+		}
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/82/2061421e1abb001711b8bf632416c20d

@@ -0,0 +1,161 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				float dist = getDistance();
+				
+				if (dist == -1) dist =  255;
+				
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance(Pose p, double angle) {
+		Pose tmppose = new Pose(p.getX(), p.getY(), p.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (p.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		} 
+		return mindist;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+			Pose tmppose = new Pose(pose.getX(), pose.getY(), (float) (pose.getHeading() + ang));
+			float dist = map.range(tmppose);
+			if (dist == -1) dist =  255;
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/82/400ab8f526bb001711b8bf632416c20d

@@ -0,0 +1,162 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+rand.nextGaussian()*4+Math.random()*2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (angle  = ini; angle <= end; angle += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/85/4044b06d22bb001711b8bf632416c20d

@@ -0,0 +1,120 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/86/30721f2c22bb001711b8bf632416c20d

@@ -0,0 +1,120 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/88/c058977828bb001711b8bf632416c20d

@@ -0,0 +1,343 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		String s = "LCPRobot";
+
+		Object[] possibleValues = { s, robotv, robotbt };
+		Object robot = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
+				possibleValues, possibleValues[0]);
+		
+		boolean result = false;
+		if (robot == s) {
+			robot = new LCPRobot();
+		}
+		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/90/0000f60a19bb001711b8bf632416c20d

@@ -0,0 +1,24 @@
+package config;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class Models {
+	private LineMap map;
+
+	public Models(LineMap map) {
+		this.map = map;
+	}
+
+	public double expectedSonarRead(Pose p, double angle) {
+		Pose tmppose = new Pose(p.getX(), p.getY(), p.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 15;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (p.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		} 
+		return mindist;
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/91/00cbb01c22bb001711b8bf632416c20d

@@ -0,0 +1,120 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/92/600820f122bb001711b8bf632416c20d

@@ -0,0 +1,130 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.NXTRegulatedMotor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(200);
+		scannerm.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		scannerm.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		scannerm.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			scannerm.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/92/80862cca21bb001711b8bf632416c20d

@@ -0,0 +1,121 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		//provider = new OdometryPoseProvider(pilot);
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/94/70f2c8e021bb001711b8bf632416c20d

@@ -0,0 +1,122 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		//provider = new OdometryPoseProvider(pilot);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/95/d072bf9626bb001711b8bf632416c20d

@@ -0,0 +1,162 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+rand.nextGaussian()*4+Math.random()*2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/97/2044bf0325bb001711b8bf632416c20d

@@ -0,0 +1,340 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.print("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+		if (data.getDistance() == 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { robotlcp, 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/97/20ba409023bb001711b8bf632416c20d

@@ -0,0 +1,127 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			scannerm.rotate(-(j * interval));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/9b/00b56b1827bb001711b8bf632416c20d

@@ -0,0 +1,161 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+
+				data.setDistance((int) dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle + 90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo = -cone / 2; angulo <= cone / 2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist)
+				mindist = dist;
+		}
+		double v = mindist + rand.nextGaussian() * 4 + Math.random() * 2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float) -x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		for (angle = ini; angle <= end; angle += interval) {
+			DataPose data = new DataPose();
+			double dist = getDistance();
+			data.setDistance((int) dist);
+			data.setPose(pose);
+			data.setSensorAngle(angle);
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null)
+			return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/9e/00ee781921bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) {
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        double ang = 0;
+        int engine_mult = 1;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(-90*engine_mult); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(engine_mult*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (engine_mult*end); j+=(engine_mult*interval)){ 
+            Motor.C.rotate(-(engine_mult*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            
+            DataPose data = new DataPose();
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(0);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/9f/706891f320bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        double ang = 0;
+        double engine_mult = 1;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(-90*engine_mult); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(engine_mult*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (engine_mult*end); j+=(engine_mult*interval)){ 
+            Motor.C.rotate(-(engine_mult*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            
+            DataPose data = new DataPose();
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/a0/c0387e3121bb001711b8bf632416c20d

@@ -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);
+		scanner.setPose(p);
+		if (data.getDistance() == 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		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/a2/80ecc49f20bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        int i=0;
+        double ang = 0;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            DataPose data = new DataPose();
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		// TODO Auto-generated method stub
+		
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/a3/1011551525bb001711b8bf632416c20d

@@ -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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.print("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+		// 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);
+		Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { robotlcp, 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/a3/f0e2d6fb19bb001711b8bf632416c20d

@@ -0,0 +1,162 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			int ang = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+				
+
+				Pose tmppose = new Pose(pose.getX(), pose.getY(), (float) (pose.getHeading() + ang));
+				float dist = getRead(tmppose, angle)
+				
+				if (dist == -1) dist =  255;
+				
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(ang+90);
+
+				rr.robotData(data);
+
+				ang += add;
+				if (ang == -180 || ang == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getRead(Pose p, double angle) {
+		Pose tmppose = new Pose(p.getX(), p.getY(), p.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (p.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		} 
+		return mindist;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+			Pose tmppose = new Pose(pose.getX(), pose.getY(), (float) (pose.getHeading() + ang));
+			float dist = map.range(tmppose);
+			if (dist == -1) dist =  255;
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/a4/e071a0e320bb001711b8bf632416c20d

@@ -0,0 +1,122 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        double ang = 0;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            DataPose data = new DataPose();
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/a5/b0f59b8e21bb001711b8bf632416c20d

@@ -0,0 +1,121 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		//provider = new OdometryPoseProvider(pilot);
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/a6/001f17bc27bb001711b8bf632416c20d

@@ -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/a7/c038912b1ebb001711b8bf632416c20d

@@ -0,0 +1,305 @@
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.Point;
+import java.awt.event.MouseEvent;
+import java.awt.event.MouseListener;
+import java.awt.event.MouseMotionListener;
+import java.awt.event.MouseWheelEvent;
+import java.awt.event.MouseWheelListener;
+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 MapImage extends JPanel implements MouseWheelListener, MouseListener, MouseMotionListener  {
+    private double zoom = 2.0; // pixel per cm
+    private double grid = 10.0; // cm
+    private double centerx = 0.0;
+    private double centery = 0.0; // cm
+    private Point mousePt;
+    private ArrayList<Pose> lista_pontos;
+    private ArrayList<Pose> lista_ultra;
+    private int visual_method = 0;
+    private boolean line = false;
+    
+    private Semaphore semaphore;
+    
+    private LineMap map;
+    
+	public MapImage() {
+		super();
+		semaphore = new Semaphore(1);
+		lista_pontos = new ArrayList<Pose>();
+		lista_ultra = new ArrayList<Pose>();
+		setBackground(Color.BLACK);
+		addMouseWheelListener(this);
+		addMouseListener(this);
+		addMouseMotionListener(this);
+	}
+	
+	public MapImage(LineMap map) {
+		this();
+		this.map = map;
+	}
+	
+    private void drawModel (Graphics g) {
+        int width = (int) (getWidth()+2*centerx);
+        int height = (int) (getHeight()+2*centery);
+        int count = 0;
+        int x_tmp = 0, y_tmp = 0;
+        
+	    	for (Pose p : lista_pontos) {
+			double hading = Math.toRadians(p.getHeading());
+				
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		
+			if (visual_method == 0) {
+				g.setColor(Color.getHSBColor((float) (hading/(2.0*Math.PI)), 1, 1));
+				g.fillOval(
+						x-(int)(zoom/2.0*1.5),
+						y-(int)(zoom/2.0*1.5),
+						(int)(zoom*1.5),
+						(int)(zoom*1.5)
+				);
+			} else if (visual_method == 1) {
+	            g.setColor(Color.RED);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+	
+	           g.drawLine(
+	                width/2+(int)(p.getX()*zoom+zoom*Math.sin(hading)),
+	                height/2-(int)(p.getY()*zoom-zoom*Math.cos(hading)),
+	                width/2+(int)(p.getX()*zoom+0.6*zoom*Math.sin(Math.PI/8+hading)),
+	                height/2-(int)(p.getY()*zoom-0.6*zoom*Math.cos(Math.PI/8+hading))
+	            );
+			} else if (visual_method == 2) {
+				g.setColor(Color.RED);
+				g.fillOval(
+						x-(int)(zoom/2.0*1.5),
+						y-(int)(zoom/2.0*1.5),
+						(int)(zoom*1.5),
+						(int)(zoom*1.5)
+				);
+	            g.setColor(Color.BLACK);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+			}
+	
+		    	if (line && count != 0) {
+		    		g.setColor(Color.LIGHT_GRAY);
+		    		g.drawLine(x_tmp, y_tmp, x, y);
+		    	}
+	
+		    	x_tmp = x;
+		    	y_tmp = y;
+		    	count++;
+		}
+	    	
+	    	g.setColor(Color.RED);
+	    	for (Pose p : lista_ultra) {
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		g.fillRect(
+					x-(int)(zoom/2.0*1.0),
+					y-(int)(zoom/2.0*1.0),
+					(int)(zoom*1.0),
+					(int)(zoom*1.0)
+			);
+	    	}
+	    	
+	    	if (map != null) {
+	    		Line[] lines = map.getLines();
+	    		for (int i = 0; i < lines.length; i++) {
+	    			Line l = lines[i];
+	            g.drawLine(
+		                width/2+(int)(l.x1*zoom),
+		                height/2-(int)(l.y1*zoom), 
+		                width/2+(int)(l.x2*zoom),
+		                height/2-(int)(l.y2*zoom)
+		        );
+	    		}
+	    	}
+    }
+    
+    @Override
+    protected void paintComponent(Graphics g) {
+        int width = (int) (getWidth());
+        int height = (int) (getHeight());
+        int width2 = (int) (getWidth()+2*centerx);
+        int height2 = (int) (getHeight()+2*centery);
+        super.paintComponent(g);
+    
+        g.setColor(new Color(20, 20, 20));
+        
+        int initial_x = height2/2;
+        while (initial_x < width) {
+        	initial_x += grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        initial_x = height2/2;
+        while (initial_x > 0) {
+        	initial_x -= grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        int initial_y = width2/2;
+        while (initial_y < width) {
+        	initial_y += grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+        initial_y = width2/2;
+        while (initial_y > 0) {
+        	initial_y -= grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+
+        g.setColor(Color.ORANGE);
+        g.drawLine(width2/2, 0, width2/2, height);
+        g.drawLine(0, height2/2, width, height2/2);
+
+        if (semaphore.tryAcquire()) {
+	        drawModel(g);
+	        semaphore.release();
+		}
+    }
+    
+    /**
+     * Adiciona um ponto ao mapa
+     * @param p ponto
+     */
+    public void addPoint(Pose p) {
+    		if (semaphore.tryAcquire()) {
+    			lista_pontos.clear();
+    			lista_pontos.add(p);
+	        semaphore.release();
+		}
+    		repaint();
+	}
+
+    public void addPoint(float x, float y, float z) {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.add(new Pose(x, y, z));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addRead(float x, float y) {
+		if (semaphore.tryAcquire()) {
+			lista_ultra.add(new Pose(x, y, 0));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addPoint(double x, double y, double z) {
+    		addPoint((float)x, (float)y, (float)z);
+	}
+    
+
+    public void addRead(double x, double y) {
+    		addRead((float)x, (float)y);
+	}
+    
+    
+    public void showLine () {
+    		line = !line;
+    		repaint();
+    }
+
+    public void setVisual (int method) {
+    		visual_method = method;
+    		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.");
+		}
+    }
+    
+	public void clean() {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.clear();
+			lista_ultra.clear();
+	        semaphore.release();
+		}
+		repaint();
+	}
+	@Override
+	public void mouseDragged(MouseEvent e) {
+		centerx += e.getX() - mousePt.x;
+		centery += e.getY() - mousePt.y;
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseMoved(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseClicked(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mousePressed(MouseEvent e) {
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseReleased(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseEntered(MouseEvent e) {
+	}
+	
+	@Override
+	public void mouseExited(MouseEvent e) {	
+	}
+	
+	@Override
+	public void mouseWheelMoved(MouseWheelEvent e) {
+		if(e.getWheelRotation()<0){
+			if (zoom < 15.0)
+				zoom *= 1.1;
+			repaint();
+		}
+		//Zoom out
+		if(e.getWheelRotation()>0){
+			if (zoom > 1.0)
+				zoom /= 1.1;
+			repaint();
+		}
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/ab/105c265221bb001711b8bf632416c20d

@@ -0,0 +1,338 @@
+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.LCPRobot;
+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);
+		scanner.setPose(p);
+		if (data.getDistance() == 255) {
+			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);
+		//Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { robotlcp, 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/ae/d08de8371ebb001711b8bf632416c20d

@@ -0,0 +1,305 @@
+import java.awt.Color;
+import java.awt.Graphics;
+import java.awt.Point;
+import java.awt.event.MouseEvent;
+import java.awt.event.MouseListener;
+import java.awt.event.MouseMotionListener;
+import java.awt.event.MouseWheelEvent;
+import java.awt.event.MouseWheelListener;
+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 MapImage extends JPanel implements MouseWheelListener, MouseListener, MouseMotionListener  {
+    private double zoom = 2.0; // pixel per cm
+    private double grid = 10.0; // cm
+    private double centerx = 0.0;
+    private double centery = 0.0; // cm
+    private Point mousePt;
+    private ArrayList<Pose> lista_pontos;
+    private ArrayList<Pose> lista_ultra;
+    private int visual_method = 0;
+    private boolean line = false;
+    
+    private Semaphore semaphore;
+    
+    private LineMap map;
+    
+	public MapImage() {
+		super();
+		semaphore = new Semaphore(1);
+		lista_pontos = new ArrayList<Pose>();
+		lista_ultra = new ArrayList<Pose>();
+		setBackground(Color.BLACK);
+		addMouseWheelListener(this);
+		addMouseListener(this);
+		addMouseMotionListener(this);
+	}
+	
+	public MapImage(LineMap map) {
+		this();
+		this.map = map;
+	}
+	
+    private void drawModel (Graphics g) {
+        int width = (int) (getWidth()+2*centerx);
+        int height = (int) (getHeight()+2*centery);
+        int count = 0;
+        int x_tmp = 0, y_tmp = 0;
+        
+	    	for (Pose p : lista_pontos) {
+			double hading = Math.toRadians(p.getHeading());
+				
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		
+			if (visual_method == 0) {
+				g.setColor(Color.getHSBColor((float) (hading/(2.0*Math.PI)), 1, 1));
+				g.fillOval(
+						x-(int)(zoom/2.0*1.5),
+						y-(int)(zoom/2.0*1.5),
+						(int)(zoom*1.5),
+						(int)(zoom*1.5)
+				);
+			} else if (visual_method == 1) {
+	            g.setColor(Color.WHITE);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+	
+	           g.drawLine(
+	                width/2+(int)(p.getX()*zoom+zoom*Math.sin(hading)),
+	                height/2-(int)(p.getY()*zoom-zoom*Math.cos(hading)),
+	                width/2+(int)(p.getX()*zoom+0.6*zoom*Math.sin(Math.PI/8+hading)),
+	                height/2-(int)(p.getY()*zoom-0.6*zoom*Math.cos(Math.PI/8+hading))
+	            );
+			} else if (visual_method == 2) {
+				g.setColor(Color.WHITE);
+				g.fillOval(
+						x-(int)(zoom/2.0*1.5),
+						y-(int)(zoom/2.0*1.5),
+						(int)(zoom*1.5),
+						(int)(zoom*1.5)
+				);
+	            g.setColor(Color.BLACK);
+	            g.drawLine(
+	                width/2+(int)(p.getX()*zoom),
+	                height/2-(int)(p.getY()*zoom), 
+	                width/2+(int)(p.getX()*zoom+Math.sin(hading)*zoom),
+	                height/2-(int)(p.getY()*zoom-Math.cos(hading)*zoom)
+	            );
+			}
+	
+		    	if (line && count != 0) {
+		    		g.setColor(Color.LIGHT_GRAY);
+		    		g.drawLine(x_tmp, y_tmp, x, y);
+		    	}
+	
+		    	x_tmp = x;
+		    	y_tmp = y;
+		    	count++;
+		}
+	    	
+	    	g.setColor(Color.RED);
+	    	for (Pose p : lista_ultra) {
+	    		int x = width/2+(int)(p.getX()*zoom);
+	    		int y = height/2+(int)(p.getY()*zoom)*-1;
+	    		g.fillRect(
+					x-(int)(zoom/2.0*1.0),
+					y-(int)(zoom/2.0*1.0),
+					(int)(zoom*1.0),
+					(int)(zoom*1.0)
+			);
+	    	}
+	    	
+	    	if (map != null) {
+	    		Line[] lines = map.getLines();
+	    		for (int i = 0; i < lines.length; i++) {
+	    			Line l = lines[i];
+	            g.drawLine(
+		                width/2+(int)(l.x1*zoom),
+		                height/2-(int)(l.y1*zoom), 
+		                width/2+(int)(l.x2*zoom),
+		                height/2-(int)(l.y2*zoom)
+		        );
+	    		}
+	    	}
+    }
+    
+    @Override
+    protected void paintComponent(Graphics g) {
+        int width = (int) (getWidth());
+        int height = (int) (getHeight());
+        int width2 = (int) (getWidth()+2*centerx);
+        int height2 = (int) (getHeight()+2*centery);
+        super.paintComponent(g);
+    
+        g.setColor(new Color(20, 20, 20));
+        
+        int initial_x = height2/2;
+        while (initial_x < width) {
+        	initial_x += grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        initial_x = height2/2;
+        while (initial_x > 0) {
+        	initial_x -= grid*zoom;
+        	g.drawLine(0, initial_x, width, initial_x); 
+        }
+        int initial_y = width2/2;
+        while (initial_y < width) {
+        	initial_y += grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+        initial_y = width2/2;
+        while (initial_y > 0) {
+        	initial_y -= grid*zoom;
+            g.drawLine(initial_y, 0, initial_y, height);
+        }
+
+        g.setColor(Color.ORANGE);
+        g.drawLine(width2/2, 0, width2/2, height);
+        g.drawLine(0, height2/2, width, height2/2);
+
+        if (semaphore.tryAcquire()) {
+	        drawModel(g);
+	        semaphore.release();
+		}
+    }
+    
+    /**
+     * Adiciona um ponto ao mapa
+     * @param p ponto
+     */
+    public void addPoint(Pose p) {
+    		if (semaphore.tryAcquire()) {
+    			lista_pontos.clear();
+    			lista_pontos.add(p);
+	        semaphore.release();
+		}
+    		repaint();
+	}
+
+    public void addPoint(float x, float y, float z) {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.add(new Pose(x, y, z));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addRead(float x, float y) {
+		if (semaphore.tryAcquire()) {
+			lista_ultra.add(new Pose(x, y, 0));
+	        semaphore.release();
+		}
+    		repaint();
+	}
+    
+
+    public void addPoint(double x, double y, double z) {
+    		addPoint((float)x, (float)y, (float)z);
+	}
+    
+
+    public void addRead(double x, double y) {
+    		addRead((float)x, (float)y);
+	}
+    
+    
+    public void showLine () {
+    		line = !line;
+    		repaint();
+    }
+
+    public void setVisual (int method) {
+    		visual_method = method;
+    		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.");
+		}
+    }
+    
+	public void clean() {
+		if (semaphore.tryAcquire()) {
+			lista_pontos.clear();
+			lista_ultra.clear();
+	        semaphore.release();
+		}
+		repaint();
+	}
+	@Override
+	public void mouseDragged(MouseEvent e) {
+		centerx += e.getX() - mousePt.x;
+		centery += e.getY() - mousePt.y;
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseMoved(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseClicked(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mousePressed(MouseEvent e) {
+		mousePt = e.getPoint();
+		repaint();
+		
+	}
+	@Override
+	public void mouseReleased(MouseEvent e) {		
+	}
+	
+	@Override
+	public void mouseEntered(MouseEvent e) {
+	}
+	
+	@Override
+	public void mouseExited(MouseEvent e) {	
+	}
+	
+	@Override
+	public void mouseWheelMoved(MouseWheelEvent e) {
+		if(e.getWheelRotation()<0){
+			if (zoom < 15.0)
+				zoom *= 1.1;
+			repaint();
+		}
+		//Zoom out
+		if(e.getWheelRotation()>0){
+			if (zoom > 1.0)
+				zoom /= 1.1;
+			repaint();
+		}
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/af/107fedec1ebb001711b8bf632416c20d

@@ -0,0 +1,162 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+rand.nextGaussian()*4+Math.random()*2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/af/70f1c4e724bb001711b8bf632416c20d

@@ -0,0 +1,338 @@
+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.LCPRobot;
+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);
+		scanner.setPose(p);
+		if (data.getDistance() == 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { robotlcp, 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/b/10df563521bb001711b8bf632416c20d

@@ -0,0 +1,338 @@
+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.LCPRobot;
+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);
+		scanner.setPose(p);
+		if (data.getDistance() == 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		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/b3/90ddfa3b25bb001711b8bf632416c20d

@@ -0,0 +1,340 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.print("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { robotlcp, 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/b7/20d90e9423bb001711b8bf632416c20d

@@ -0,0 +1,127 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			scannerm.rotate(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/b7/402b781e27bb001711b8bf632416c20d

@@ -0,0 +1,161 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+
+				data.setDistance((int) dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle + 90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo = -cone / 2; angulo <= cone / 2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist)
+				mindist = dist;
+		}
+		double v = mindist + rand.nextGaussian() * 4 + Math.random() * 2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float) -x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		for (angle = ini; angle <= end; angle += interval) {
+			DataPose data = new DataPose();
+			double dist = getDistance();
+			data.setDistance((int) dist);
+			data.setPose(pose);
+			data.setSensorAngle(angle+90);
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null)
+			return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/bf/5011ee2d1abb001711b8bf632416c20d

@@ -0,0 +1,161 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				float d = getDistance();
+				
+				if (dist == -1) dist =  255;
+				
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		} 
+		return mindist;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+			Pose tmppose = new Pose(pose.getX(), pose.getY(), (float) (pose.getHeading() + ang));
+			float dist = map.range(tmppose);
+			if (dist == -1) dist =  255;
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/c2/30394cec22bb001711b8bf632416c20d

@@ -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();
+			
+			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/c2/90de08f21ebb001711b8bf632416c20d

@@ -0,0 +1,162 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 20;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+rand.nextGaussian()*4+Math.random()*2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/c4/b0fd50d51dbb001711b8bf632416c20d

@@ -0,0 +1,159 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+Random.nextGaussian();
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/c5/c0f227881bbb001711b8bf632416c20d

@@ -0,0 +1,158 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		// limites 222 e 4
+		return mindist+Math.random()*10;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/c7/c07e5c2a1dbb001711b8bf632416c20d

@@ -0,0 +1,24 @@
+package config;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class Models {
+	private LineMap map;
+
+	public Models(LineMap map) {
+		this.map = map;
+	}
+
+	public double expectedSonarRead(Pose p, double angle) {
+		Pose tmppose = new Pose(p.getX(), p.getY(), p.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (p.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		} 
+		return mindist;
+	}
+}

.metadata/.plugins/org.eclipse.core.resources/.history/c8/f0c93a6f24bb001711b8bf632416c20d

@@ -0,0 +1,126 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			System.out.println(j);
+			scannerm.rotateTo(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotateTo(0);
+		return result;
+	}
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/cc/1093155f1bbb001711b8bf632416c20d

@@ -0,0 +1,160 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				if (dist == -1) dist =  255;
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		// limites 222 e 4
+		return mindist;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			if (dist == -1) dist =  255;
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/ce/208f695b28bb001711b8bf632416c20d

@@ -0,0 +1,340 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { /*robotlcp,*/ 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/ce/507d46f826bb001711b8bf632416c20d

@@ -0,0 +1,159 @@
+package robots;
+
+import java.util.ArrayList;
+import java.util.Random;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+	private Random rand;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		double v = mindist+rand.nextGaussian()*4+Math.random()*2;
+		v = Math.min(255, v);
+		return v;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+		rand = new Random();
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		for (angle  = ini; angle <= end; angle += interval) {
+			DataPose data = new DataPose();
+			double dist = getDistance();
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(angle);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/ce/60f35ce920bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        double ang = 0;
+        double engine_mult = 1;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(-90*engine_mult); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(engine_mult*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            
+            DataPose data = new DataPose();
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/d2/104e2e4025bb001711b8bf632416c20d

@@ -0,0 +1,125 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+		pose = new Pose();
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			System.out.println(j);
+			scannerm.rotateTo(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(j);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotateTo(0);
+		return result;
+	}
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/d6/e000ef7925bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+		pose = new Pose();
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			scannerm.rotateTo(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(j);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotateTo(0);
+		return result;
+	}
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/d7/f00d863c22bb001711b8bf632416c20d

@@ -0,0 +1,119 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose = new Pose(x, y, a);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/d8/3092ef9928bb001711b8bf632416c20d

@@ -0,0 +1,348 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		String s = "LCPRobot";
+
+		Object[] possibleValues = { s, robotv, robotbt };
+		Object robottmp = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
+				possibleValues, possibleValues[0]);
+		Robot robot;
+		
+		
+		boolean result = false;
+		if (robottmp == s) {
+			robot = new LCPRobot();
+		} else {
+			robot = robottmp;
+		}
+
+		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/df/505f451d21bb001711b8bf632416c20d

@@ -0,0 +1,122 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		provider = new OdometryPoseProvider(pilot);
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/e/1097c38d21bb001711b8bf632416c20d

@@ -0,0 +1,120 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+
+	public LCPRobot() {
+		pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		provider = new OdometryPoseProvider(pilot);
+	}
+
+	@Override
+	public void move(double x) {
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		Motor.C.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			Motor.C.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(provider.getPose());
+			result.add(data);
+		}
+		Motor.C.setSpeed(200);
+		Motor.C.rotate(0);
+		return result;
+	}
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/e4/a066c48523bb001711b8bf632416c20d

@@ -0,0 +1,129 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(200);
+		scannerm.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
+		scannerm.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
+		scannerm.setSpeed(80);
+		for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
+			scannerm.rotate(-(engine_mult * interval));
+			val = sonar.getDistance();
+			// ang = (j/(5*end)) * 180.0;
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/e6/f05382a120bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        int i=0;
+        double ang = 0;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            DataPose data = new DataPose();
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+		
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/e9/a0138df227bb001711b8bf632416c20d

@@ -0,0 +1,340 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		//Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { /*robotlcp,*/ 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/ed/6059681420bb001711b8bf632416c20d

@@ -0,0 +1,148 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+    //private Pose pose;
+    //private Simulate simthread;
+    private RobotReturn rr;
+    private LineMap map;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+    }
+
+    static double theta = 0;
+    static double x = 0;
+    static double y = 0;
+
+    @Override
+    public void move(double x) {
+        pilot.travel(x);
+        x += Math.cos(Math.toRadians(theta)) * x;
+        y += Math.sin(Math.toRadians(theta)) * x;
+        // double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+        // double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+        // pose.translate((float) dx, (float) -dy);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+        if ((theta + x) > 360){
+            int add = (theta + x) % 360;
+            theta += add;
+        }
+        else if ((theta + x) < 0){
+            theta = 360 + (theta + x);
+        }
+        else
+            theta += x;
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        Pose p = new Pose((float) x, (float) y, (float) theta);
+        int val = 0;
+        int i=0;
+        double ang = 0;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            DataPose data = new DataPose();
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(p);
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveRight() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveBackward() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stop() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		// TODO Auto-generated method stub
+		
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/f/50fd429224bb001711b8bf632416c20d

@@ -0,0 +1,126 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+		pose = new Pose();
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			System.out.println(j);
+			scannerm.rotateTo(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotateTo(0);
+		return result;
+	}
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/f0/90e0a2cb26bb001711b8bf632416c20d

@@ -0,0 +1,340 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		Robot robotlcp = new LCPRobot();
+
+		Object[] possibleValues = { robotlcp, 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/f2/e0cfed6c28bb001711b8bf632416c20d

@@ -0,0 +1,341 @@
+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.LCPRobot;
+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.print(p);
+		System.out.print("   sang:"+data.getSensorAngle());
+		System.out.println("   distance:"+data.getDistance());
+		imap.addPoint(p);
+		scanner.setPose(p);
+
+		// 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);
+		
+		if (data.getDistance() != 255) {
+			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);
+		String s = "LCPRobot";
+
+		Object[] possibleValues = { s, robotv, robotbt };
+		Object robot = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
+				possibleValues, possibleValues[0]);
+		
+		boolean result = false;
+		if (robot == s) robot = new LCPRobot();
+		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/f3/9086e0fc20bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        double ang = 0;
+        double engine_mult = 1;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(-90*engine_mult); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(engine_mult*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (engine_mult*end); j+=(engine_mult*interval)){ 
+            Motor.C.rotate(-(engine_mult*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            
+            DataPose data = new DataPose();
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(0);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/f4/10e5606c1bbb001711b8bf632416c20d

@@ -0,0 +1,160 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+
+public class VirtualRobot implements Robot {
+	private Pose pose;
+	private Simulate simthread;
+	private RobotReturn rr;
+	private LineMap map;
+	private int angle = 0;
+
+	private class Simulate extends Thread {
+		public boolean run = true;
+
+		public void run() {
+			angle = 0;
+			int add = -5;
+			while (run) {
+				DataPose data = new DataPose();
+
+				double dist = getDistance();
+				if (dist == -1) dist =  255;
+				
+				data.setDistance((int)dist);
+				data.setPose(pose);
+				data.setSensorAngle(angle+90);
+
+				rr.robotData(data);
+
+				angle += add;
+				if (angle == -180 || angle == 0)
+					add *= -1;
+
+				try {
+					Thread.sleep(50);
+				} catch (InterruptedException e) {
+				}
+			}
+		}
+
+	}
+	
+	public double getDistance() {
+		Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
+		float mindist = Float.POSITIVE_INFINITY;
+		int cone = 30;
+		for (int angulo=-cone/2; angulo <= cone/2; angulo++) {
+			tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
+			float dist = map.range(tmppose);
+			if (dist > 0 && dist < mindist) mindist = dist;
+		}
+		// limites 222 e 4
+		return mindist+Math.random()*10;
+	}
+
+	public VirtualRobot(LineMap map) {
+		pose = new Pose();
+		pose.setHeading(0);
+		this.map = map;
+	}
+
+	@Override
+	public void moveForward() {
+		move(5);
+	}
+
+	@Override
+	public void moveLeft() {
+		pose.rotateUpdate(45);
+	}
+
+	@Override
+	public void moveRight() {
+		pose.rotateUpdate(-45);
+	}
+
+	@Override
+	public void moveBackward() {
+		move(-5);
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void stop() {
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int ang = 0;
+		for (ang  = ini; ang <= end; ang += interval) {
+			DataPose data = new DataPose();
+
+			double dist = getDistance();
+			if (dist == -1) dist =  255;
+			
+			data.setDistance((int)dist);
+			data.setPose(pose);
+			data.setSensorAngle(ang);	
+			result.add(data);
+		}
+		return result;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		stopScann();
+		simthread = new Simulate();
+		simthread.start();
+	}
+
+	@Override
+	public void stopScann() {
+		if (simthread == null) return;
+		simthread.run = false;
+		try {
+			simthread.join();
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+	}
+
+	@Override
+	public void disconnect() {
+
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+	
+	@Override
+	public String toString() {
+		return "Virtual Robot";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/f5/103ad60521bb001711b8bf632416c20d

@@ -0,0 +1,124 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) {
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        double ang = 0;
+        double engine_mult = 1;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(-90*engine_mult); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(engine_mult*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (engine_mult*end); j+=(engine_mult*interval)){ 
+            Motor.C.rotate(-(engine_mult*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            
+            DataPose data = new DataPose();
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(0);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/f7/a0e545ba20bb001711b8bf632416c20d

@@ -0,0 +1,122 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+    		pilot.travel(x);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        int val = 0;
+        double ang = 0;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            DataPose data = new DataPose();
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(provider.getPose());
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+		
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		provider.setPose(new Pose(x, y, a));
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/fa/605b209620bb001711b8bf632416c20d

@@ -0,0 +1,141 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.localization.OdometryPoseProvider;
+import lejos.robotics.mapping.LineMap;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.util.Delay;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+
+public class LCPRobot implements Robot {
+	OdometryPoseProvider provider;
+
+    static DifferentialPilot pilot;
+    static UltrasonicSensor sonar;
+
+    public LCPRobot () {
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+        sonar = new UltrasonicSensor(SensorPort.S1);
+        pilot.setTravelSpeed(10);
+        pilot.setRotateSpeed(40);
+        provider = new OdometryPoseProvider(pilot);
+    }
+
+    @Override
+    public void move(double x) {
+        pilot.travel(x);
+        x += Math.cos(Math.toRadians(theta)) * x;
+        y += Math.sin(Math.toRadians(theta)) * x;
+        // double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+        // double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+        // pose.translate((float) dx, (float) -dy);
+    }
+
+    @Override
+    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
+        pilot.rotate(x);
+        if ((theta + x) > 360){
+            int add = (theta + x) % 360;
+            theta += add;
+        }
+        else if ((theta + x) < 0){
+            theta = 360 + (theta + x);
+        }
+        else
+            theta += x;
+    }
+
+    @Override
+    public ArrayList<DataPose> scann(int ini, int end, int interval) {
+        ArrayList<DataPose> result = new ArrayList<DataPose>();
+        Pose p = new Pose((float) x, (float) y, (float) theta);
+        int val = 0;
+        int i=0;
+        double ang = 0;
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
+        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
+        Motor.C.setSpeed(80);
+        for (int j=0; j <= (5*end); j+=(5*interval)){ 
+            DataPose data = new DataPose();
+            Motor.C.rotate(-(5*interval));
+            val = sonar.getDistance();
+            //ang = (j/(5*end)) * 180.0;
+            ang = ini + interval;
+            data.setDistance(val);
+            data.setSensorAngle(ang);
+            data.setPose(p);
+            result.add(data);
+        }
+        Motor.C.setSpeed(200);
+        Motor.C.rotate(460);
+        return result;
+    }
+   
+    @Override 
+    public String toString() {
+        return "LCP Robot";
+    }
+
+	@Override
+	public void moveForward() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveLeft() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveRight() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void moveBackward() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stop() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public boolean connect() {
+		return true;
+	}
+
+	@Override
+	public void disconnect() {		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		// TODO Auto-generated method stub
+		
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/ff/30860f3024bb001711b8bf632416c20d

@@ -0,0 +1,127 @@
+package robots;
+
+import java.util.ArrayList;
+
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.remote.RemoteMotor;
+
+public class LCPRobot implements Robot {
+	private Pose pose;
+	static DifferentialPilot pilot;
+	static UltrasonicSensor sonar;
+	RemoteMotor scannerm = Motor.A;
+	RemoteMotor ma = Motor.B;
+	RemoteMotor mb = Motor.C;
+
+	public LCPRobot() {
+
+	}
+
+	@Override
+	public void move(double x) {
+		double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
+		double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
+		pose.translate((float) dx, (float) -dy);
+		pilot.travel(x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		pose.rotateUpdate((float)-x);
+		pilot.rotate(x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		ArrayList<DataPose> result = new ArrayList<DataPose>();
+		int val = 0;
+		double ang = 0;
+		int engine_mult = 1;
+		scannerm.setSpeed(80);
+		for (int j = ini; j <= end; j += interval) {
+			System.out.println(j);
+			scannerm.rotateTo(-(j * engine_mult));
+			scannerm.waitComplete();
+			val = sonar.getDistance();
+			ang = ini + interval;
+
+			DataPose data = new DataPose();
+			data.setDistance(val);
+			data.setSensorAngle(ang);
+			data.setPose(pose);
+			result.add(data);
+		}
+		scannerm.setSpeed(200);
+		scannerm.rotate(0);
+		return result;
+	}
+	
+	
+
+	@Override
+	public String toString() {
+		return "LCP Robot";
+	}
+
+	@Override
+	public void moveForward() {
+		pilot.forward();
+	}
+
+	@Override
+	public void moveLeft() {
+		pilot.rotateLeft();
+	}
+
+	@Override
+	public void moveRight() {
+		pilot.rotateRight();
+	}
+
+	@Override
+	public void moveBackward() {
+		pilot.backward();
+	}
+
+	@Override
+	public void stop() {
+		pilot.stop();
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public void stopScann() {
+		// TODO Auto-generated method stub
+
+	}
+
+	@Override
+	public boolean connect() {
+		//pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
+		pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		pilot.setTravelSpeed(10);
+		pilot.setRotateSpeed(40);
+		return true;
+	}
+
+	@Override
+	public void disconnect() {
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		pose.setHeading(a);
+		pose.setLocation(x, y);
+	}
+
+}