Arrumando detalhes

.metadata/.log

@@ -1231,3 +1231,127 @@ 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)
+!SESSION 2017-10-26 18:34:08.301 -----------------------------------------------
+eclipse.buildId=4.7.1.M20171009-0410
+java.version=1.8.0_20
+java.vendor=Oracle Corporation
+BootLoader constants: OS=macosx, ARCH=x86_64, WS=cocoa, NL=en_US
+Framework arguments:  -product org.eclipse.epp.package.java.product -keyring /Users/gabrielcapella/.eclipse_keyring
+Command-line arguments:  -os macosx -ws cocoa -arch x86_64 -product org.eclipse.epp.package.java.product -keyring /Users/gabrielcapella/.eclipse_keyring
+
+!ENTRY org.eclipse.epp.logging.aeri.ide 2 17 2017-10-26 18:34:18.052
+!MESSAGE Server ‘org.eclipse.epp.logging.aeri.ide.server’ failed with exception: Connect to dev.eclipse.org:443 [dev.eclipse.org/198.41.30.200] failed: Host is down. ; version: 2.0.6.v20170906-1226
+!STACK 0
+org.apache.http.conn.HttpHostConnectException: Connect to dev.eclipse.org:443 [dev.eclipse.org/198.41.30.200] failed: Host is down
+	at org.apache.http.impl.conn.DefaultHttpClientConnectionOperator.connect(DefaultHttpClientConnectionOperator.java:158)
+	at org.apache.http.impl.conn.PoolingHttpClientConnectionManager.connect(PoolingHttpClientConnectionManager.java:353)
+	at org.apache.http.impl.execchain.MainClientExec.establishRoute(MainClientExec.java:380)
+	at org.apache.http.impl.execchain.MainClientExec.execute(MainClientExec.java:236)
+	at org.apache.http.impl.execchain.ProtocolExec.execute(ProtocolExec.java:184)
+	at org.apache.http.impl.execchain.RetryExec.execute(RetryExec.java:88)
+	at org.apache.http.impl.execchain.RedirectExec.execute(RedirectExec.java:110)
+	at org.apache.http.impl.client.InternalHttpClient.doExecute(InternalHttpClient.java:184)
+	at org.apache.http.impl.client.CloseableHttpClient.execute(CloseableHttpClient.java:82)
+	at org.apache.http.impl.client.CloseableHttpClient.execute(CloseableHttpClient.java:55)
+	at org.apache.http.client.fluent.Request.internalExecute(Request.java:173)
+	at org.apache.http.client.fluent.Executor.execute(Executor.java:262)
+	at org.eclipse.epp.internal.logging.aeri.ide.server.mars.IO.request(IO.java:170)
+	at org.eclipse.epp.internal.logging.aeri.ide.server.mars.IO.refreshConfiguration(IO.java:64)
+	at org.eclipse.epp.internal.logging.aeri.ide.server.mars.ServerConnection.startUp(ServerConnection.java:124)
+	at com.google.common.util.concurrent.AbstractIdleService$DelegateService$1.run(AbstractIdleService.java:62)
+	at com.google.common.util.concurrent.Callables$4.run(Callables.java:122)
+	at java.lang.Thread.run(Thread.java:745)
+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.ssl.SSLConnectionSocketFactory.connectSocket(SSLConnectionSocketFactory.java:337)
+	at org.apache.http.impl.conn.DefaultHttpClientConnectionOperator.connect(DefaultHttpClientConnectionOperator.java:141)
+	... 17 more
+
+!ENTRY org.eclipse.equinox.p2.transport.ecf 4 1002 2017-10-26 19:18:08.776
+!MESSAGE Unable to connect to repository http://www.lejos.org/tools/eclipse/plugin/nxj/content.xml
+!STACK 0
+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:82)
+	at org.eclipse.ecf.provider.filetransfer.httpclient4.HttpClientFileSystemBrowser.runRequest(HttpClientFileSystemBrowser.java:263)
+	at org.eclipse.ecf.provider.filetransfer.browse.AbstractFileSystemBrowser$DirectoryJob.run(AbstractFileSystemBrowser.java:69)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.eclipse.equinox.p2.transport.ecf 4 1002 2017-10-26 19:18:08.800
+!MESSAGE Unable to connect to repository http://download.eclipse.org/oomph/updates/milestone/latest/compositeContent.xml
+!STACK 0
+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:82)
+	at org.eclipse.ecf.provider.filetransfer.httpclient4.HttpClientFileSystemBrowser.runRequest(HttpClientFileSystemBrowser.java:263)
+	at org.eclipse.ecf.provider.filetransfer.browse.AbstractFileSystemBrowser$DirectoryJob.run(AbstractFileSystemBrowser.java:69)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.eclipse.equinox.p2.transport.ecf 4 1002 2017-10-26 19:18:08.805
+!MESSAGE Unable to connect to repository http://download.eclipse.org/eclipse/updates/4.7/compositeContent.xml
+!STACK 0
+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:82)
+	at org.eclipse.ecf.provider.filetransfer.httpclient4.HttpClientFileSystemBrowser.runRequest(HttpClientFileSystemBrowser.java:263)
+	at org.eclipse.ecf.provider.filetransfer.browse.AbstractFileSystemBrowser$DirectoryJob.run(AbstractFileSystemBrowser.java:69)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)
+
+!ENTRY org.eclipse.equinox.p2.transport.ecf 4 1002 2017-10-26 19:18:09.094
+!MESSAGE Unable to connect to repository http://download.eclipse.org/releases/oxygen/compositeContent.xml
+!STACK 0
+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:82)
+	at org.eclipse.ecf.provider.filetransfer.httpclient4.HttpClientFileSystemBrowser.runRequest(HttpClientFileSystemBrowser.java:263)
+	at org.eclipse.ecf.provider.filetransfer.browse.AbstractFileSystemBrowser$DirectoryJob.run(AbstractFileSystemBrowser.java:69)
+	at org.eclipse.core.internal.jobs.Worker.run(Worker.java:56)

.metadata/.plugins/org.eclipse.core.resources/.history/0/607d0a7c31b80017145c9b6064c27648

@@ -1,112 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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 = distance/10;
-    			//System.out.println(d);
-    			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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		double d = h*2.0/25.0/20.0;
-		double a = -oval_size/2;
-		for (SonarRead r: lista_leituras) {
-			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/10/70c258f4a4ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/11/10083bb3a5ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/12/200fd8fb1eb80017145c9b6064c27648

@@ -1,53 +0,0 @@
-package robots;
-
-import lejos.robotics.mapping.LineMap;
-
-public class VirtualRobot implements Robot {
-
-	public VirtualRobot(LineMap map) {
-		// TODO Auto-generated constructor stub
-	}
-
-	@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 boolean connect(RobotReturn r) {
-		// TODO Auto-generated method stub
-		return false;
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/12/e0c1a89b8eba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/67/2001f1e6dbb90017124d9f37712eb7c1 → .metadata/.plugins/org.eclipse.core.resources/.history/14/806d736491ba001711b8bf632416c20d

@@ -17,6 +17,8 @@ public class BluetoothRobot implements Robot {
 	private NXTComm nxtComm;
 	private RobotReturn rr;
 	private Semaphore semaphore;
+	private Semaphore countsema;
+	private int creads = 1;
 	
 	public static final byte FORWARD = 0;
 	public static final byte STOP = 1;
@@ -29,11 +31,16 @@ public class BluetoothRobot implements Robot {
 	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;
+	
+	private static final int scannangle = 5;
 
 	private DataOutputStream output;
 	private DataInputStream input;
-//	private Send sendthread;
 	private Receiver receivethread;
+	
+	private ArrayList<DataPose> reads;
 		
 	private class Receiver extends Thread {
 		public boolean run = true;
@@ -59,9 +66,14 @@ public class BluetoothRobot implements Robot {
 						DataPose d = new DataPose();
 						d.setDistance(distance);
 						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
+						d.setPose(new Pose(x, y, alpha-90));
+						if (rr != null)
+							rr.robotData(d);	
+						
+						if (reads != null && reads.size() < creads) {
+							reads.add(d);
+						}
 						
-						rr.robotData(d);						
 					} catch (IOException e1) {
 						continue;
 					}
@@ -81,12 +93,27 @@ public class BluetoothRobot implements Robot {
 			semaphore.release();
 		}
 	}
-
-	private void send(int command, float f) {
+	
+	private void send(int command, int i) {
 		if (semaphore.tryAcquire()) {
 			try {
 				output.write(command);
-				output.writeFloat(f);
+				output.write(i);
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+	
+	private void send(Pose p) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(SETPOSE);
+				output.writeFloat(p.getX());
+				output.writeFloat(p.getY());
+				output.writeFloat(p.getHeading());
 				output.flush();
 			} catch (IOException e) {
 				System.out.println("Erro ao enviar...");
@@ -97,6 +124,8 @@ public class BluetoothRobot implements Robot {
 	
 	public BluetoothRobot (String name) {
 		semaphore = new Semaphore(1);
+		countsema = new Semaphore(1);
+		receivethread = new Receiver();
 		this.name = name;
 	}
 
@@ -136,6 +165,7 @@ public class BluetoothRobot implements Robot {
 			
 			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
 			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
+			send(SETSCANANGLE, scannangle); // vai scanear em 5 em 5 graus
 		} catch (NXTCommException e) {
 			return false;
 		}
@@ -158,35 +188,41 @@ public class BluetoothRobot implements Robot {
 
 	@Override
 	public void rotate(double x) {
-		// TODO Auto-generated method stub
-		
+		send(ROTATE, (float)x);
 	}
 
 	@Override
 	public ArrayList<DataPose> scann(int ini, int end, int interval) {
-		// TODO Auto-generated method stub
-		return null;
+		send(SETSCANANGLE, interval);
+		creads = 180/interval;
+		reads = new ArrayList<DataPose>();
+		scann(null);
+		
+		while(reads.size() <= creads) { };
+		
+		stopScann();
+		creads = -1;
+		return reads;
 	}
 
 	@Override
 	public void scann(RobotReturn r) {
 		rr = r;
+		send(STARTSCANN);
 	}
 
 	@Override
 	public void stopScann() {
-		// TODO Auto-generated method stub
-		
+		send(STOPSCANN);
 	}
 
 	@Override
 	public void disconnect() {
-		sendthread.send(EXIT);
+		send(EXIT);
+		if (receivethread == null) return;
 		receivethread.run = false;
-		sendthread.send(EXIT);
 		try {
 			receivethread.join();
-			sendthread.join();
 		} catch (InterruptedException e1) {
 			System.out.println("Nao foi possivel finalizar as threads...");
 		}
@@ -201,8 +237,13 @@ public class BluetoothRobot implements Robot {
 
 	@Override
 	public void setPose(float x, float y, float a) {
-		// TODO Auto-generated method stub
-		
+		send(new Pose(x, y, a));
+	}
+	
+	
+	@Override
+	public String toString() {
+		return "Bluetooth Mestre/Escravo";
 	}
 
 }

.metadata/.plugins/org.eclipse.core.resources/.history/17/1089c19fcbb90017124d9f37712eb7c1

@@ -1,222 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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);
-		
-
-		boolean result = false;
-		if (robot != null) {
-			result = robot.connect();
-			robot.start(this);
-		}
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		 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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\n";
-		 text += "<arrows> - Move robot.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 'g')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		else if (input == 's')
-			setRobotPose();
-		
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	private void setRobotPose() {
-		String xs = JOptionPane.showInputDialog("Enter x (cm):");
-		String ys = JOptionPane.showInputDialog("Enter y (cm):");
-		String as = JOptionPane.showInputDialog("Enter heading (degress):");
-		float x  = Float.parseFloat(xs);
-		float y  = Float.parseFloat(ys);
-		float a  = Float.parseFloat(as);
-		robot.setPose(x, y, a);
-	}
-
-	@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();
-		
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

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

@@ -1,206 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		
-		 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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@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();
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/1a/a0151c13a4ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/1a/a07dcaea92ba001711b8bf632416c20d

@@ -0,0 +1,177 @@
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+
+import lejos.nxt.Motor;
+import lejos.nxt.NXTRegulatedMotor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.Sound;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.comm.BTConnection;
+import lejos.nxt.comm.Bluetooth;
+import lejos.robotics.navigation.DifferentialPilot;
+import lejos.robotics.navigation.Pose;
+import lejos.robotics.localization.*;
+
+class Scanner extends Thread {
+	DataOutputStream output;
+	OdometryPoseProvider provider;
+	boolean run;
+	public boolean scann;
+	public int increment = 5;
+	
+	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) ;
+		int position = 0;
+		
+		while (run) {
+			if (scann == false) {
+				scannerm.rotateTo(0);
+				continue;
+			}
+			if (position == 90 || position == -90)
+				increment *= -1;
+
+			scannerm.rotateTo(position);
+			scannerm.waitComplete();
+			
+			int distance = sensor.getDistance();
+						
+			Pose pose = provider.getPose();
+			float x = pose.getX();
+			float y = pose.getY();
+			float alpha = pose.getHeading();
+			
+			if (distance != 255) {
+				try {
+					output.write('@');
+					output.write(position+90);
+					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.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/1a/b0c84325acba001711b8bf632416c20d

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

@@ -1,89 +0,0 @@
-package robots;
-
-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 class Simulate extends Thread {
-		public boolean run = true;
-		public void run() {
-			int ang = 0;
-			int add = 5;
-			while(run) {
-				System.out.println(ang);
-				DataPose data = new DataPose();
-				data.setDistance(100);
-				data.setPose(pose);
-				data.setSensorAngle(ang);
-				
-				rr.robotData(data);
-				
-				ang += add;
-				if (ang == -90 || ang == 90) add *= -1;
-				
-				try {
-					Thread.sleep(100);
-				} catch (InterruptedException e) {
-				}
-			}
-		}
-		
-	}
-
-	public VirtualRobot(LineMap map) {
-		simthread = new Simulate();
-		pose = new Pose();
-	}
-
-	@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 boolean connect(RobotReturn r) {
-		rr = r;
-		simthread.start();
-		return true;
-	}
-
-	@Override
-	public void stop() {		
-	}
-
-	@Override
-	public void exit() {
-		simthread.run = false;
-		try {
-			simthread.join();
-		} catch (InterruptedException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/1c/20b11b80a8ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/1c/c0786a72a5ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/71/c0243887ccb90017124d9f37712eb7c1 → .metadata/.plugins/org.eclipse.core.resources/.history/1d/308fa0529fba001711b8bf632416c20d

@@ -3,14 +3,14 @@ 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.ButtonGroup;
 import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
 import javax.swing.JOptionPane;
 import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
 import javax.swing.JSplitPane;
 import javax.swing.SwingUtilities;
 
@@ -41,7 +41,6 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 	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());
@@ -61,34 +60,25 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		splitPane.setOneTouchExpandable(true);
 		splitPane.setDividerLocation((int) (frame.getHeight() / 2));
 		frame.add(splitPane);
-		
-
-		boolean result = false;
-		if (robot != null) {
-			result = robot.connect();
-			robot.scann(this);
-		}
 
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		 showHelp();
+		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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\n";
-		 text += "<arrows> - Move robot.\n";
-		 JOptionPane.showMessageDialog(null, text, "HELP", JOptionPane.PLAIN_MESSAGE);
+		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) {
@@ -97,49 +87,155 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 
 	@Override
 	public void keyPressed(KeyEvent e) {
+
 		char input = e.getKeyChar();
-		if (input == '1')
+		switch (input) {
+		case '1':
 			imap.setVisual(0);
-		else if (input == '2')
+			break;
+		case '2':
 			imap.setVisual(1);
-		else if (input == '3')
+			break;
+		case '3':
 			imap.setVisual(2);
-		else if (input == 'l')
+			break;
+		case 'l':
 			imap.showLine();
-		else if (input == 'g')
+			break;
+		case 'g':
 			imap.save();
-		else if (input == 'c')
+			break;
+		case 'f':
+			scanner.save();
+			break;
+		case 'c':
 			imap.clean();
-		else if (input == 's')
+			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);
+
+		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());
+			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() {
-		String xs = JOptionPane.showInputDialog("Enter x (cm):");
-		String ys = JOptionPane.showInputDialog("Enter y (cm):");
-		String as = JOptionPane.showInputDialog("Enter heading (degress):");
-		float x  = Float.parseFloat(xs);
-		float y  = Float.parseFloat(ys);
-		float a  = Float.parseFloat(as);
-		robot.setPose(x, y, a);
+		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
@@ -194,9 +290,12 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 	public void robotData(DataPose data) {
 		// posicao do robo
 		Pose p = data.getPose();
-		
+
 		imap.addPoint(p);
-		if (data.getDistance() == 255) return;
+		if (data.getDistance() == 255) {
+			scanner.setPose(p);
+			return;
+		}
 
 		// ponto do ultrasonico
 		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
@@ -212,12 +311,19 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		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]);
+		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() {

.metadata/.plugins/org.eclipse.core.resources/.history/1d/50193f6bc6b90017124d9f37712eb7c1

@@ -1,206 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		
-		 showHelp();
-	}
-
-	private void showHelp() {
-		String text = "1,2,3 - Change global map view\n";
-		 text += "s \t\t Set Pose\n";
-		 text += "l - Show global map trace.\n";
-		 text += "c - Clean global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@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();
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/1d/50c61970c6b90017124d9f37712eb7c1

@@ -1,206 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		
-		 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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@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();
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/1d/c0369cb78eba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/1f/705f6c47c5b90017124d9f37712eb7c1

@@ -1,204 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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");
-		
-
-	    JMenuBar bar = new JMenuBar();
-	    JMenu menu = new JMenu("Options");
-	    menu.setMnemonic(KeyEvent.VK_O);
-
-	    bar.add(menu);
-	    frame.setJMenuBar(bar);
-
-
-		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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@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();
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/2/50ff40f19dba001711b8bf632416c20d

@@ -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);
+
+				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(90);
+		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/20/20af339020b80017145c9b6064c27648

@@ -1,60 +0,0 @@
-package robots;
-
-import lejos.robotics.mapping.LineMap;
-
-public class VirtualRobot implements Robot {
-	private Pose pose;
-	
-	private class Simulate extends Thread {
-		public void run() {
-			
-		}
-	}
-
-	public VirtualRobot(LineMap map) {
-		// TODO Auto-generated constructor stub
-	}
-
-	@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 boolean connect(RobotReturn r) {
-		// TODO Auto-generated method stub
-		return true;
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/20/80f4cd6218b80017145c9b6064c27648

@@ -1,269 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener {
-
-	private MapImage imap;
-	
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-
-	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;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						
-						// posicao do robo
-						imap.addPoint(x, y, Math.toRadians(alpha+90));
-						
-						// ponto do ultrasonico
-						double sensor_ang = Math.toRadians(alpha+angle);
-						double dx = Math.cos(sensor_ang)*distance;
-						double dy = Math.sin(sensor_ang)*distance;
-						imap.addRead(x+dx, y+dy);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	public MainProgram (LineMap map) {
-		JFrame frame = new JFrame("Mapa MAC0318");
-
-
-		receivethread= new Receiver();
-		sendthread = new Send();
-		
-//		output = out;
-//		input = in;
-
-//		receivethread.start();
-//		sendthread.start();
-		
-		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-        frame.setLayout(new BorderLayout());
-        imap = new MapImage(map);
-        ScannerImage scanner = new ScannerImage();
-//		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);
-		
-//		
-//		String text = "1,2,3 - Change view mode.\n";
-//		text += "s - Save image.\n";
-//		text += "l - Show trace.\n";
-//		text += "c - Clean map.\n";
-//		JOptionPane.showMessageDialog(null, text);
-	}
-	
-    public void addPoint(Pose p) {
-    		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1') imap.setVisual(0);
-		else if (input == '2') imap.setVisual(1);
-		else if (input == '3') imap.setVisual(2);
-		else if (input == 'l') imap.showLine();
-		else if (input == 's') imap.save();
-		else if (input == 'c') imap.clean();
-		
-		 switch(e.getKeyCode()) { 
-	        case KeyEvent.VK_UP:
-	        		sendthread.send(FORWARD);
-	            break;
-	        case KeyEvent.VK_DOWN:
-	        		sendthread.send(BACKWARD);
-	            break;
-	        case KeyEvent.VK_LEFT:
-	        		sendthread.send(LEFT);
-	            break;
-	        case KeyEvent.VK_RIGHT :
-	        		sendthread.send(RIGHT);
-	            break;
-	    }			 
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {		
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		
-		
-//		NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH); 
-//		NXTInfo[] nxtInfo = nxtComm.search("NXT8"); //find brick with NXT_ID by doing a Bluetooth inquiry
-//		if (nxtInfo.length == 0) { // failed to find a brick with the ID
-//			System.err.println("NO NXT found");
-//			System.exit(1);
-//		}
-//		if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-//			System.err.println("Failed to open NXT");
-//			System.exit(1);
-//		}
-//		
-//		final DataInputStream input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-//		final DataOutputStream output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-//		
-		SwingUtilities.invokeLater(new Runnable() {
-            public void run() {
-            	//new Map(input, output);
-
-            	new MainProgram(map);
-            }
-        });
-		
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-		
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		
-	}
-
-	@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) {		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/22/b0dc773632b80017145c9b6064c27648

@@ -1,114 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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 = distance;
-    			//System.out.println(d);
-    			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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		int distance = h*2/25;
-		double d = distance/20.0;
-		double a = -oval_size/4;
-		for (SonarRead r: lista_leituras) {
-			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-//			x += a*Math.sin(Math.toRadians(r.ang+90));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/23/104082ed20b80017145c9b6064c27648

@@ -1,67 +0,0 @@
-package robots;
-
-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 class Simulate extends Thread {
-		public boolean run = true;
-		public void run() {
-			while(run) {
-				
-			}
-		}
-		
-	}
-
-	public VirtualRobot(LineMap map) {
-		simthread = new Simulate();
-	}
-
-	@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 boolean connect(RobotReturn r) {
-		rr = r;
-		simthread.start();
-		return true;
-	}
-
-	@Override
-	public void stop() {		
-	}
-
-	@Override
-	public void exit() {
-		simthread.run = false;
-		simthread.join();
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/23/301d2c7da6ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/23/30cca097a4ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/24/d0f6db9aa0ba001711b8bf632416c20d

@@ -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(90);
+		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/25/60f61ae129b80017145c9b6064c27648

@@ -1,88 +0,0 @@
-package robots;
-
-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 class Simulate extends Thread {
-		public boolean run = true;
-		public void run() {
-			int ang = 0;
-			int add = 5;
-			while(run) {
-				DataPose data = new DataPose();
-				data.setDistance(20);
-				data.setPose(pose);
-				data.setSensorAngle(ang);
-				
-				rr.robotData(data);
-				
-				ang += add;
-				if (ang == -90 || ang == 90) add *= -1;
-				
-				try {
-					Thread.sleep(100);
-				} catch (InterruptedException e) {
-				}
-			}
-		}
-		
-	}
-
-	public VirtualRobot(LineMap map) {
-		simthread = new Simulate();
-		pose = new Pose();
-	}
-
-	@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 boolean connect(RobotReturn r) {
-		rr = r;
-		simthread.start();
-		return true;
-	}
-
-	@Override
-	public void stop() {		
-	}
-
-	@Override
-	public void exit() {
-		simthread.run = false;
-		try {
-			simthread.join();
-		} catch (InterruptedException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/26/30f7b5eac1b90017124d9f37712eb7c1

@@ -1,179 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	private RobotReturn rr;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-		
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						rr.robotData(d);						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	public BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect (RobotReturn r) {
-		rr = r;
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/28/d032e1488eba001711b8bf632416c20d

@@ -0,0 +1,269 @@
+package robots;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.concurrent.Semaphore;
+
+import lejos.pc.comm.NXTComm;
+import lejos.pc.comm.NXTCommException;
+import lejos.pc.comm.NXTCommFactory;
+import lejos.pc.comm.NXTInfo;
+import lejos.robotics.navigation.Pose;
+
+public class BluetoothRobot implements Robot {
+	private String name;
+	private NXTComm nxtComm;
+	private RobotReturn rr;
+	private Semaphore semaphore;
+	private Semaphore countsema;
+	private int creads = 1;
+	
+	public static final byte FORWARD = 0;
+	public static final byte STOP = 1;
+	public static final byte EXIT = 2;
+	public static final byte LEFT = 3;
+	public static final byte RIGHT = 4;
+	public static final byte BACKWARD = 5;
+	
+	public static final byte STOPSCANN = 6;
+	public static final byte STARTSCANN = 7;
+	public static final byte MOVE = 8;
+	public static final byte ROTATE = 9;
+	public static final byte SETPOSE = 10;
+	public static final byte SETSCANANGLE = 11;
+	
+	private static final int scannangle = 5;
+
+	private DataOutputStream output;
+	private DataInputStream input;
+	private Receiver receivethread;
+	
+	private ArrayList<DataPose> reads;
+		
+	private class Receiver extends Thread {
+		public boolean run = true;
+		@Override
+		public void run() {
+			int bytes_valiable = -1;
+			
+			while(run) {
+				try {
+					bytes_valiable = input.available();
+				} catch (IOException e1) {
+					// TODO Auto-generated catch block
+					e1.printStackTrace();
+				}
+				if (bytes_valiable >= 0) {
+					try {
+						if (input.readByte() != '@') continue;
+						int angle = input.readByte();
+						float alpha = input.readFloat();
+						float x = input.readFloat();
+						int distance = input.readByte();
+						float y = input.readFloat();
+						DataPose d = new DataPose();
+						d.setDistance(distance);
+						d.setSensorAngle(angle);
+						d.setPose(new Pose(x, y, alpha-90));
+						if (rr != null)
+							rr.robotData(d);	
+						if (reads.size() <= creads) {
+							reads.add(d);
+						}
+						countsema.release(180/creads);
+						
+					} catch (IOException e1) {
+						continue;
+					}
+				}
+			}
+		}
+	}
+
+	private void send(int command) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(command);
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+
+	private void send(int command, float f) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(command);
+				output.writeFloat(f);
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+	
+	private void send(int command, int i) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(command);
+				output.write(i);
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+	
+	private void send(Pose p) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(SETPOSE);
+				output.writeFloat(p.getX());
+				output.writeFloat(p.getY());
+				output.writeFloat(p.getHeading());
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+	
+	public BluetoothRobot (String name) {
+		reads = new ArrayList<DataPose>();
+		semaphore = new Semaphore(1);
+		countsema = new Semaphore(180);
+		receivethread = new Receiver();
+		this.name = name;
+	}
+
+	@Override
+	public void moveForward() {
+		send(FORWARD);
+	}
+
+	@Override
+	public void moveLeft() {
+		send(LEFT);
+	}
+
+	@Override
+	public void moveRight() {
+		send(RIGHT);
+	}
+
+	@Override
+	public void moveBackward() {
+		send(BACKWARD);
+	}
+
+	@Override
+	public boolean connect () {
+		try {
+			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
+			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
+			if (nxtInfo.length == 0) { // failed to find a brick with the ID
+				System.err.println("NO NXT found");
+				return false;
+			}
+			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
+				System.err.println("Failed to open NXT");
+				return false;
+			}
+			
+			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
+			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
+			send(SETSCANANGLE, scannangle); // vai scanear em 5 em 5 graus
+		} catch (NXTCommException e) {
+			return false;
+		}
+		
+		receivethread.start();
+		
+		return true;	
+	}
+
+	@Override
+	public void stop() {
+		send(STOP);
+	}
+
+
+	@Override
+	public void move(double x) {
+		send(MOVE, (float)x);
+	}
+
+	@Override
+	public void rotate(double x) {
+		send(ROTATE, (float)x);
+	}
+
+	@Override
+	public ArrayList<DataPose> scann(int ini, int end, int interval) {
+		send(SETSCANANGLE, interval);
+		
+		creads = 180/interval;
+		reads.clear();
+		send(SETSCANANGLE, scannangle);
+		try {
+			scann(null);
+			countsema.acquire(180);
+		} catch (InterruptedException e1) {
+			// TODO Auto-generated catch block
+			e1.printStackTrace();
+		}
+		stopScann();
+		countsema.release(180);
+		creads = -1;
+		return null;
+	}
+
+	@Override
+	public void scann(RobotReturn r) {
+		rr = r;
+		send(STARTSCANN);
+	}
+
+	@Override
+	public void stopScann() {
+		send(STOPSCANN);
+	}
+
+	@Override
+	public void disconnect() {
+		send(EXIT);
+		if (receivethread == null) return;
+		receivethread.run = false;
+		try {
+			receivethread.join();
+		} catch (InterruptedException e1) {
+			System.out.println("Nao foi possivel finalizar as threads...");
+		}
+		try {
+			nxtComm.close();
+		} catch (IOException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}
+		
+	}
+
+	@Override
+	public void setPose(float x, float y, float a) {
+		send(new Pose(x, y, a));
+	}
+	
+	
+	@Override
+	public String toString() {
+		return "Bluetooth Mestre/Escravo";
+	}
+
+}

.metadata/.plugins/org.eclipse.core.resources/.history/29/10b4e4650db9001711c7a09600ea5d68

@@ -1,185 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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;
-
-	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);
-		// 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);
-		
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-		
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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();
-		imap.addPoint(p);
-		
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle()+p.getHeading());
-		double dx = Math.cos(sensor_ang)*data.getDistance();
-		double dy = Math.sin(sensor_ang)*data.getDistance();
-		imap.addRead(p.getX()+dx, p.getY()+dy);
-		double expected = SonarModel();
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/2b/c0680f0ecbb90017124d9f37712eb7c1

@@ -1,220 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		 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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\n";
-		 text += "<arrows> - Move robot.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 'g')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		else if (input == 's')
-			setRobotPose();
-		
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	private void setRobotPose() {
-		String xs = JOptionPane.showInputDialog("Enter x (m):");
-		String ys = JOptionPane.showInputDialog("Enter y (m):");
-		String as = JOptionPane.showInputDialog("Enter heading (degress):");
-		float x  = Float.parseFloat(xs);
-		float y  = Float.parseFloat(ys);
-		float a  = Float.parseFloat(as);
-		robot.setPose(x, y, a);
-	}
-
-	@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();
-		
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/2b/d09491ff07b9001711c7a09600ea5d68

@@ -1,113 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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/100;
-    			
-    			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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		int distance = h*2/25;
-		double d = distance/20.0;
-		double a = -oval_size/4;
-		for (SonarRead r: lista_leituras) {
-			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/2c/50b57accaaba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/dc/e0629a13dfb90017124d9f37712eb7c1 → .metadata/.plugins/org.eclipse.core.resources/.history/2d/50598ff190ba001711b8bf632416c20d

@@ -17,12 +17,15 @@ class Scanner extends Thread {
 	DataOutputStream output;
 	OdometryPoseProvider provider;
 	boolean run;
+	public boolean scann;
+	public int increment = 5;
 	
 	Scanner (DataOutputStream output, OdometryPoseProvider provider) {
 		super();
 		this.output = output;
 		this.provider = provider;
 		run = true;
+		scann = false;
 	}
 	
 	public void stop () {
@@ -32,10 +35,13 @@ class Scanner extends Thread {
 	public void run() {
 		NXTRegulatedMotor scannerm = Motor.A;
 		UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1) ;
-		int increment = 5;
 		int position = 0;
 		
 		while (run) {
+			if (scann == false) {
+				scannerm.rotateTo(0);
+				continue;
+			}
 			if (position == 90 || position == -90)
 				increment *= -1;
 
@@ -79,6 +85,13 @@ public class Sonar {
 	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();
@@ -127,6 +140,30 @@ public class Sonar {
 				case BACKWARD:
 					pilot.backward();
 					break;
+				case STOPSCANN:
+					scan.scann = false;
+					break;
+				case STARTSCANN:
+					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();
+					provider.setPose(new Pose(x,y,a));
+					break;
+				case SETSCANANGLE:
+					int i = input.readByte();
+					scan.increment = i;
+					break;
 			}
 		}
 		Sound.beep();

.metadata/.plugins/org.eclipse.core.resources/.history/2d/b0888cb01ab80017145c9b6064c27648

@@ -1,60 +0,0 @@
-package robots;
-
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-
-public class VirtualRobot implements Robot {
-
-	public VirtualRobot(LineMap map) {
-		// TODO Auto-generated constructor stub
-	}
-
-	@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 connect() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public Pose getData() {
-		// TODO Auto-generated method stub
-		return null;
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/2e/4015b9dc0db9001711c7a09600ea5d68

@@ -1,186 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import config.SonarModel;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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;
-
-	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);
-		// 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);
-		
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-		
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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();
-		imap.addPoint(p);
-		
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle()+p.getHeading());
-		double dx = Math.cos(sensor_ang)*data.getDistance();
-		double dy = Math.sin(sensor_ang)*data.getDistance();
-		imap.addRead(p.getX()+dx, p.getY()+dy);
-		double expected = SonarModel.expectedRead();
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/2f/306eeab11eb80017145c9b6064c27648

@@ -1,181 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.bluetooth.BluetoothConnectionException;
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-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;
-
-	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);
-		ScannerImage scanner = new ScannerImage();
-		// 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);
-		
-		if (robot != null)
-			robot.connect(this);
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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) {
-		// TODO Auto-generated method stub
-		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/2f/80b91ae231b80017145c9b6064c27648

@@ -1,114 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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 = distance;
-    			//System.out.println(d);
-    			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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		int distance = h*2/25;
-		double d = distance/20.0;
-		double a = -oval_size/2;
-		for (SonarRead r: lista_leituras) {
-			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-			x += a*Math.sin(Math.toRadians(r.ang));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/3/60f5422f19b80017145c9b6064c27648

@@ -1,59 +0,0 @@
-package robots;
-
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	
-	BluetoothRobot (String name) {
-		
-	}
-
-	@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 connect() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public Pose getData() {
-		// TODO Auto-generated method stub
-		return null;
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/3/80234ffba4ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/3/f018d8e820b80017145c9b6064c27648

@@ -1,64 +0,0 @@
-package robots;
-
-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 class Simulate extends Thread {
-		public void run() {
-			
-		}
-	}
-
-	public VirtualRobot(LineMap map) {
-		simthread = new Simulate();
-	}
-
-	@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 boolean connect(RobotReturn r) {
-		rr = r;
-		simthread.start();
-		return true;
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/7/a0365be0dbb90017124d9f37712eb7c1 → .metadata/.plugins/org.eclipse.core.resources/.history/33/208d50b7a1ba001711b8bf632416c20d

@@ -17,6 +17,7 @@ public class BluetoothRobot implements Robot {
 	private NXTComm nxtComm;
 	private RobotReturn rr;
 	private Semaphore semaphore;
+	private int creads = 1;
 	
 	public static final byte FORWARD = 0;
 	public static final byte STOP = 1;
@@ -29,11 +30,16 @@ public class BluetoothRobot implements Robot {
 	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;
+	
+	private static final int scannangle = 5;
 
 	private DataOutputStream output;
 	private DataInputStream input;
-//	private Send sendthread;
 	private Receiver receivethread;
+	
+	private ArrayList<DataPose> reads;
 		
 	private class Receiver extends Thread {
 		public boolean run = true;
@@ -59,9 +65,10 @@ public class BluetoothRobot implements Robot {
 						DataPose d = new DataPose();
 						d.setDistance(distance);
 						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
+						d.setPose(new Pose(-y, x, alpha));
+						if (rr != null)
+							rr.robotData(d);	
 						
-						rr.robotData(d);						
 					} catch (IOException e1) {
 						continue;
 					}
@@ -95,8 +102,37 @@ public class BluetoothRobot implements Robot {
 		}
 	}
 	
+	private void send(int command, int i) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(command);
+				output.write(i);
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+	
+	private void send(Pose p) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(SETPOSE);
+				output.writeFloat(p.getX());
+				output.writeFloat(p.getY());
+				output.writeFloat(p.getHeading());
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+	
 	public BluetoothRobot (String name) {
 		semaphore = new Semaphore(1);
+		receivethread = new Receiver();
 		this.name = name;
 	}
 
@@ -136,6 +172,7 @@ public class BluetoothRobot implements Robot {
 			
 			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
 			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
+			send(SETSCANANGLE, scannangle); // vai scanear em 5 em 5 graus
 		} catch (NXTCommException e) {
 			return false;
 		}
@@ -153,40 +190,46 @@ public class BluetoothRobot implements Robot {
 
 	@Override
 	public void move(double x) {
-		send(MOVE, (float)x)
+		send(MOVE, (float)x);
 	}
 
 	@Override
 	public void rotate(double x) {
-		// TODO Auto-generated method stub
-		
+		send(ROTATE, (float)x);
 	}
 
 	@Override
 	public ArrayList<DataPose> scann(int ini, int end, int interval) {
-		// TODO Auto-generated method stub
+//		send(SETSCANANGLE, interval);
+//		creads = 180/interval;
+//		reads = new ArrayList<DataPose>();
+//		scann(null);
+//		
+//		while(reads.size() <= creads) { };
+//		
+//		stopScann();
+//		creads = -1;
 		return null;
 	}
 
 	@Override
 	public void scann(RobotReturn r) {
 		rr = r;
+		send(STARTSCANN);
 	}
 
 	@Override
 	public void stopScann() {
-		// TODO Auto-generated method stub
-		
+		send(STOPSCANN);
 	}
 
 	@Override
 	public void disconnect() {
-		sendthread.send(EXIT);
+		send(EXIT);
+		if (receivethread == null) return;
 		receivethread.run = false;
-		sendthread.send(EXIT);
 		try {
 			receivethread.join();
-			sendthread.join();
 		} catch (InterruptedException e1) {
 			System.out.println("Nao foi possivel finalizar as threads...");
 		}
@@ -201,8 +244,13 @@ public class BluetoothRobot implements Robot {
 
 	@Override
 	public void setPose(float x, float y, float a) {
-		// TODO Auto-generated method stub
-		
+		send(new Pose(-x, y, a));
+	}
+	
+	
+	@Override
+	public String toString() {
+		return "Bluetooth Mestre/Escravo";
 	}
 
 }

.metadata/.plugins/org.eclipse.core.resources/.history/96/20165cdfdbb90017124d9f37712eb7c1 → .metadata/.plugins/org.eclipse.core.resources/.history/33/b08bfb86a2ba001711b8bf632416c20d

@@ -17,6 +17,7 @@ public class BluetoothRobot implements Robot {
 	private NXTComm nxtComm;
 	private RobotReturn rr;
 	private Semaphore semaphore;
+	private int creads = 1;
 	
 	public static final byte FORWARD = 0;
 	public static final byte STOP = 1;
@@ -29,11 +30,16 @@ public class BluetoothRobot implements Robot {
 	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;
+	
+	private static final int scannangle = 5;
 
 	private DataOutputStream output;
 	private DataInputStream input;
-//	private Send sendthread;
 	private Receiver receivethread;
+	
+	private ArrayList<DataPose> reads;
 		
 	private class Receiver extends Thread {
 		public boolean run = true;
@@ -59,9 +65,10 @@ public class BluetoothRobot implements Robot {
 						DataPose d = new DataPose();
 						d.setDistance(distance);
 						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
+						d.setPose(new Pose(y, -x, alpha));
+						if (rr != null)
+							rr.robotData(d);	
 						
-						rr.robotData(d);						
 					} catch (IOException e1) {
 						continue;
 					}
@@ -95,8 +102,37 @@ public class BluetoothRobot implements Robot {
 		}
 	}
 	
+	private void send(int command, int i) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(command);
+				output.write(i);
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+	
+	private void send(Pose p) {
+		if (semaphore.tryAcquire()) {
+			try {
+				output.write(SETPOSE);
+				output.writeFloat(p.getX());
+				output.writeFloat(p.getY());
+				output.writeFloat(p.getHeading());
+				output.flush();
+			} catch (IOException e) {
+				System.out.println("Erro ao enviar...");
+			}
+			semaphore.release();
+		}
+	}
+	
 	public BluetoothRobot (String name) {
 		semaphore = new Semaphore(1);
+		receivethread = new Receiver();
 		this.name = name;
 	}
 
@@ -112,13 +148,12 @@ public class BluetoothRobot implements Robot {
 
 	@Override
 	public void moveRight() {
-//		sendthread.send(RIGHT);
-		
+		send(RIGHT);
 	}
 
 	@Override
 	public void moveBackward() {
-//		sendthread.send(BACKWARD);
+		send(BACKWARD);
 	}
 
 	@Override
@@ -137,60 +172,64 @@ public class BluetoothRobot implements Robot {
 			
 			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
 			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
+			send(SETSCANANGLE, scannangle); // vai scanear em 5 em 5 graus
 		} catch (NXTCommException e) {
 			return false;
 		}
 		
 		receivethread.start();
-		sendthread.start();
 		
 		return true;	
 	}
 
 	@Override
 	public void stop() {
-		sendthread.send(STOP);
+		send(STOP);
 	}
 
 
 	@Override
 	public void move(double x) {
-		output.write(position);
-		output.writeFloat(alpha);
-		
+		send(MOVE, (float)x);
 	}
 
 	@Override
 	public void rotate(double x) {
-		// TODO Auto-generated method stub
-		
+		send(ROTATE, (float)x);
 	}
 
 	@Override
 	public ArrayList<DataPose> scann(int ini, int end, int interval) {
-		// TODO Auto-generated method stub
+//		send(SETSCANANGLE, interval);
+//		creads = 180/interval;
+//		reads = new ArrayList<DataPose>();
+//		scann(null);
+//		
+//		while(reads.size() <= creads) { };
+//		
+//		stopScann();
+//		creads = -1;
 		return null;
 	}
 
 	@Override
 	public void scann(RobotReturn r) {
 		rr = r;
+		send(STARTSCANN);
 	}
 
 	@Override
 	public void stopScann() {
-		// TODO Auto-generated method stub
-		
+		send(STOPSCANN);
 	}
 
 	@Override
 	public void disconnect() {
-		sendthread.send(EXIT);
+		send(EXIT);
+		if (receivethread == null) return;
 		receivethread.run = false;
-		sendthread.send(EXIT);
 		try {
 			receivethread.join();
-			sendthread.join();
 		} catch (InterruptedException e1) {
 			System.out.println("Nao foi possivel finalizar as threads...");
 		}
@@ -205,8 +244,13 @@ public class BluetoothRobot implements Robot {
 
 	@Override
 	public void setPose(float x, float y, float a) {
-		// TODO Auto-generated method stub
-		
+		send(new Pose(y, -x, a));
+	}
+	
+	
+	@Override
+	public String toString() {
+		return "Bluetooth Mestre/Escravo";
 	}
 
 }

.metadata/.plugins/org.eclipse.core.resources/.history/35/405ba1e50db9001711c7a09600ea5d68

@@ -1,188 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import config.SonarModel;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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 SonarModel 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 = SonarModel(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);
-		
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-		
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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();
-		imap.addPoint(p);
-		
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle()+p.getHeading());
-		double dx = Math.cos(sensor_ang)*data.getDistance();
-		double dy = Math.sin(sensor_ang)*data.getDistance();
-		imap.addRead(p.getX()+dx, p.getY()+dy);
-		double expected = SonarModel.expectedRead();
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/35/905643571eb80017145c9b6064c27648

@@ -1,181 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.LinkedList;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	private RobotReturn rr;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-		
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						rr.robotData(d);						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	public BluetoothRobot (String name) {
-		this.name = name;
-		data = new LinkedList<DataPose>();
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect (RobotReturn r) {
-		rr = r;
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/37/20e590971cb80017145c9b6064c27648

@@ -1,176 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.LinkedList;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-	
-	private LinkedList<DataPose> data;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						data.add(d);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	BluetoothRobot (String name) {
-		this.name = name;
-		data = new LinkedList<DataPose>();
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect () {
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		input.close();
-		output.close();
-		
-	}
-
-	@Override
-	public DataPose getData() {
-		return data.pollFirst();
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/37/60acc8f807b9001711c7a09600ea5d68

@@ -1,113 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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*0.4;
-    			
-    			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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		int distance = h*2/25;
-		double d = distance/20.0;
-		double a = -oval_size/4;
-		for (SonarRead r: lista_leituras) {
-			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/37/90dce55b1eb80017145c9b6064c27648

@@ -1,180 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.LinkedList;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	private RobotReturn rr;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-		
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						rr.robotData(d);						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	public BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect (RobotReturn r) {
-		rr = r;
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/37/d010c503c2b90017124d9f37712eb7c1

@@ -1,179 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	private RobotReturn rr;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-		
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						rr.robotData(d);						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	public BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect (RobotReturn r) {
-		rr = r;
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/37/f04973f420b80017145c9b6064c27648

@@ -1,72 +0,0 @@
-package robots;
-
-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 class Simulate extends Thread {
-		public boolean run = true;
-		public void run() {
-			while(run) {
-				
-			}
-		}
-		
-	}
-
-	public VirtualRobot(LineMap map) {
-		simthread = new Simulate();
-	}
-
-	@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 boolean connect(RobotReturn r) {
-		rr = r;
-		simthread.start();
-		return true;
-	}
-
-	@Override
-	public void stop() {		
-	}
-
-	@Override
-	public void exit() {
-		simthread.run = false;
-		try {
-			simthread.join();
-		} catch (InterruptedException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/38/207dc9b51eb80017145c9b6064c27648

@@ -1,170 +0,0 @@
-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.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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;
-
-	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);
-		ScannerImage scanner = new ScannerImage();
-		// 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);
-		
-		if (robot != null)
-			robot.connect(this);
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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) {
-		// TODO Auto-generated method stub
-		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/38/902941f81ab80017145c9b6064c27648

@@ -1,177 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						
-						// posicao do robo
-						imap.addPoint(x, y, (float)Math.toRadians(alpha+90));
-						
-						// ponto do ultrasonico
-						double sensor_ang = Math.toRadians(alpha+angle);
-						double dx = Math.cos(sensor_ang)*distance;
-						double dy = Math.sin(sensor_ang)*distance;
-						imap.addRead(x+dx, y+dy);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@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 boolean connect () {
-		try {
-			NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public Pose getData() {
-		// TODO Auto-generated method stub
-		return null;
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/38/b012d95b1bb80017145c9b6064c27648

@@ -1,177 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						
-						// posicao do robo
-						imap.addPoint(x, y, (float)Math.toRadians(alpha+90));
-						
-						// ponto do ultrasonico
-						double sensor_ang = Math.toRadians(alpha+angle);
-						double dx = Math.cos(sensor_ang)*distance;
-						double dy = Math.sin(sensor_ang)*distance;
-						imap.addRead(x+dx, y+dy);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@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 boolean connect () {
-		try {
-			NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public DataPose getData() {
-		// TODO Auto-generated method stub
-		return null;
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/38/b075154e16b80017145c9b6064c27648

@@ -1,267 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.mapping.LineMap;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener {
-
-	private MapImage imap;
-	
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-
-	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;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						
-						// posicao do robo
-						imap.addPoint(x, y, Math.toRadians(alpha+90));
-						
-						// ponto do ultrasonico
-						double sensor_ang = Math.toRadians(alpha+angle);
-						double dx = Math.cos(sensor_ang)*distance;
-						double dy = Math.sin(sensor_ang)*distance;
-						imap.addRead(x+dx, y+dy);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	public MainProgram (LineMap map) {
-		JFrame frame = new JFrame("Mapa MAC0318");
-
-
-		receivethread= new Receiver();
-		sendthread = new Send();
-		
-//		output = out;
-//		input = in;
-
-//		receivethread.start();
-//		sendthread.start();
-		
-		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-        frame.setLayout(new BorderLayout());
-        imap = new MapImage(map);
-        ScannerImage scanner = new ScannerImage();
-//		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);
-		
-//		
-//		String text = "1,2,3 - Change view mode.\n";
-//		text += "s - Save image.\n";
-//		text += "l - Show trace.\n";
-//		text += "c - Clean map.\n";
-//		JOptionPane.showMessageDialog(null, text);
-	}
-	
-    public void addPoint(Pose p) {
-    		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1') imap.setVisual(0);
-		else if (input == '2') imap.setVisual(1);
-		else if (input == '3') imap.setVisual(2);
-		else if (input == 'l') imap.showLine();
-		else if (input == 's') imap.save();
-		else if (input == 'c') imap.clean();
-		
-		 switch(e.getKeyCode()) { 
-	        case KeyEvent.VK_UP:
-	        		sendthread.send(FORWARD);
-	            break;
-	        case KeyEvent.VK_DOWN:
-	        		sendthread.send(BACKWARD);
-	            break;
-	        case KeyEvent.VK_LEFT:
-	        		sendthread.send(LEFT);
-	            break;
-	        case KeyEvent.VK_RIGHT :
-	        		sendthread.send(RIGHT);
-	            break;
-	    }			 
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {		
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-		Robot robot = new VirtualRobot();
-		LineMap map = Map.makeMap();
-		
-		
-//		NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH); 
-//		NXTInfo[] nxtInfo = nxtComm.search("NXT8"); //find brick with NXT_ID by doing a Bluetooth inquiry
-//		if (nxtInfo.length == 0) { // failed to find a brick with the ID
-//			System.err.println("NO NXT found");
-//			System.exit(1);
-//		}
-//		if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-//			System.err.println("Failed to open NXT");
-//			System.exit(1);
-//		}
-//		
-//		final DataInputStream input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-//		final DataOutputStream output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-//		
-		SwingUtilities.invokeLater(new Runnable() {
-            public void run() {
-            	//new Map(input, output);
-
-            	new MainProgram(map);
-            }
-        });
-		
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-		
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		
-	}
-
-	@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) {		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/38/d076862516b80017145c9b6064c27648

@@ -1,267 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.mapping.LineMap;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener {
-
-	private MapImage imap;
-	
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-
-	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;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						
-						// posicao do robo
-						map.addPoint(x, y, Math.toRadians(alpha+90));
-						
-						// ponto do ultrasonico
-						double sensor_ang = Math.toRadians(alpha+angle);
-						double dx = Math.cos(sensor_ang)*distance;
-						double dy = Math.sin(sensor_ang)*distance;
-						map.addRead(x+dx, y+dy);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	public MainProgram (LineMap map) {
-		JFrame frame = new JFrame("Mapa MAC0318");
-
-
-		receivethread= new Receiver();
-		sendthread = new Send();
-		
-//		output = out;
-//		input = in;
-
-//		receivethread.start();
-//		sendthread.start();
-		
-		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-        frame.setLayout(new BorderLayout());
-        imap = new MapImage(map);
-        ScannerImage scanner = new ScannerImage();
-//		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);
-		
-//		
-//		String text = "1,2,3 - Change view mode.\n";
-//		text += "s - Save image.\n";
-//		text += "l - Show trace.\n";
-//		text += "c - Clean map.\n";
-//		JOptionPane.showMessageDialog(null, text);
-	}
-	
-    public void addPoint(Pose p) {
-    		map.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1') map.setVisual(0);
-		else if (input == '2') map.setVisual(1);
-		else if (input == '3') map.setVisual(2);
-		else if (input == 'l') map.showLine();
-		else if (input == 's') map.save();
-		else if (input == 'c') map.clean();
-		
-		 switch(e.getKeyCode()) { 
-	        case KeyEvent.VK_UP:
-	        		sendthread.send(FORWARD);
-	            break;
-	        case KeyEvent.VK_DOWN:
-	        		sendthread.send(BACKWARD);
-	            break;
-	        case KeyEvent.VK_LEFT:
-	        		sendthread.send(LEFT);
-	            break;
-	        case KeyEvent.VK_RIGHT :
-	        		sendthread.send(RIGHT);
-	            break;
-	    }			 
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {		
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-		Robot robot = new VirtualRobot();
-		LineMap map = Map.makeMap();
-		
-		
-//		NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH); 
-//		NXTInfo[] nxtInfo = nxtComm.search("NXT8"); //find brick with NXT_ID by doing a Bluetooth inquiry
-//		if (nxtInfo.length == 0) { // failed to find a brick with the ID
-//			System.err.println("NO NXT found");
-//			System.exit(1);
-//		}
-//		if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-//			System.err.println("Failed to open NXT");
-//			System.exit(1);
-//		}
-//		
-//		final DataInputStream input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-//		final DataOutputStream output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-//		
-		SwingUtilities.invokeLater(new Runnable() {
-            public void run() {
-            	//new Map(input, output);
-
-            	new MainProgram(null, null);
-            }
-        });
-		
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-		
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		
-	}
-
-	@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) {		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/39/00ab9643a4ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/3a/e0b200a0a5ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/3b/4019a8921ab80017145c9b6064c27648

@@ -1,173 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						
-						// posicao do robo
-						imap.addPoint(x, y, (float)Math.toRadians(alpha+90));
-						
-						// ponto do ultrasonico
-						double sensor_ang = Math.toRadians(alpha+angle);
-						double dx = Math.cos(sensor_ang)*distance;
-						double dy = Math.sin(sensor_ang)*distance;
-						imap.addRead(x+dx, y+dy);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@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 boolean connect () {
-		try {
-			NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				System.exit(1);
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				System.exit(1);
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public Pose getData() {
-		// TODO Auto-generated method stub
-		return null;
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/3c/b04ab53810b9001711c7a09600ea5d68

@@ -1,187 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import config.SonarModel;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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 SonarModel 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 SonarModel(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);
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@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.exit();
-
-	}
-
-	@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();
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedRead(p);
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-				new MainProgram(map, robot);
-			
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/3d/70daaaff0db9001711c7a09600ea5d68

@@ -1,190 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import config.SonarModel;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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 SonarModel 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 SonarModel(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);
-		
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-		
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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();
-		imap.addPoint(p);
-		
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle()+p.getHeading());
-		double dx = Math.cos(sensor_ang)*data.getDistance();
-		double dy = Math.sin(sensor_ang)*data.getDistance();
-		imap.addRead(p.getX()+dx, p.getY()+dy);
-		
-		double expected = smodel.expectedRead(p);
-		
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/3e/00f4e5abdbb90017124d9f37712eb7c1

@@ -1,196 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	private RobotReturn rr;
-	private Semaphore semaphore;
-	
-	public static final byte FORWARD = 0;
-	public static final byte STOP = 1;
-	public static final byte EXIT = 2;
-	public static final byte LEFT = 3;
-	public static final byte RIGHT = 4;
-	public static final byte BACKWARD = 5;
-	
-	public static final byte STOPSCANN = 6;
-	public static final byte STARTSCANN = 7;
-	public static final byte MOVE = 8;
-	public static final byte ROTATE = 9;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-//	private Send sendthread;
-	private Receiver receivethread;
-		
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						rr.robotData(d);						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private void send(int command) {
-		try {
-			output.write(command);
-			output.flush();
-		} catch (IOException e) {
-			System.out.println("Erro ao enviar...");
-		}
-	}
-	
-	public BluetoothRobot (String name) {
-		semaphore = new Semaphore(1);
-		this.name = name;
-	}
-
-	@Override
-	public void moveForward() {
-//		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-//		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-//		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-//		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect () {
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-
-	@Override
-	public void move(double x) {
-		output.write(position);
-		output.writeFloat(alpha);
-		
-	}
-
-	@Override
-	public void rotate(double x) {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public ArrayList<DataPose> scann(int ini, int end, int interval) {
-		// TODO Auto-generated method stub
-		return null;
-	}
-
-	@Override
-	public void scann(RobotReturn r) {
-		rr = r;
-	}
-
-	@Override
-	public void stopScann() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void disconnect() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-		
-	}
-
-	@Override
-	public void setPose(float x, float y, float a) {
-		// TODO Auto-generated method stub
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/3e/10dc391019b80017145c9b6064c27648

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

.metadata/.plugins/org.eclipse.core.resources/.history/3e/4052ca8620b80017145c9b6064c27648

@@ -1,53 +0,0 @@
-package robots;
-
-import lejos.robotics.mapping.LineMap;
-
-public class VirtualRobot implements Robot {
-
-	public VirtualRobot(LineMap map) {
-		// TODO Auto-generated constructor stub
-	}
-
-	@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 boolean connect(RobotReturn r) {
-		// TODO Auto-generated method stub
-		return true;
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/3e/909eacc1c9b90017124d9f37712eb7c1

@@ -1,218 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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;
-	private Pose refpose;
-
-	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;
-		refpose = new Pose();
-		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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		 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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\n";
-		 text += "<arrows> - Move robot.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 'g')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		else if (input == 's')
-			setRobotPose();
-		
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	private void setRobotPose() {
-		String xs = JOptionPane.showInputDialog("Enter x (m):");
-		String ys = JOptionPane.showInputDialog("Enter y (m):");
-		String as = JOptionPane.showInputDialog("Enter heading (degress):");
-		
-	}
-
-	@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();
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/3f/20bbea78c3b90017124d9f37712eb7c1

@@ -1,179 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	private RobotReturn rr;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-		
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						rr.robotData(d);						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	public BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect (RobotReturn r) {
-		rr = r;
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/40/0003faaea5ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/40/5049491c32b80017145c9b6064c27648

@@ -1,114 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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 = distance;
-    			//System.out.println(d);
-    			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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		int distance = h*2/25;
-		double d = distance/20.0;
-		double a = -oval_size/2;
-		for (SonarRead r: lista_leituras) {
-			double x = a/2+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-//			x += a*Math.sin(Math.toRadians(r.ang+90));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/41/f0c372e0a4ba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/43/8076c006c2b90017124d9f37712eb7c1

@@ -1,179 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	private RobotReturn rr;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-		
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						rr.robotData(d);						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	public BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect (RobotReturn r) {
-		rr = r;
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name) //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/45/60d3115e15b80017145c9b6064c27648

@@ -1,265 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener {
-
-	private MapImage map;
-	
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-
-	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;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						
-						// posicao do robo
-						map.addPoint(x, y, Math.toRadians(alpha+90));
-						
-						// ponto do ultrasonico
-						double sensor_ang = Math.toRadians(alpha+angle);
-						double dx = Math.cos(sensor_ang)*distance;
-						double dy = Math.sin(sensor_ang)*distance;
-						map.addRead(x+dx, y+dy);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	public MainProgram (DataInputStream in, DataOutputStream out) {
-		JFrame frame = new JFrame("Mapa MAC0318");
-
-
-		receivethread= new Receiver();
-		sendthread = new Send();
-		
-		output = out;
-		input = in;
-
-//		receivethread.start();
-//		sendthread.start();
-		
-		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-        frame.setLayout(new BorderLayout());
-        map = new MapImage();
-        ScannerImage scanner = new ScannerImage();
-//		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, map);
-		splitPane.setOneTouchExpandable(true);
-		splitPane.setDividerLocation((int)(frame.getHeight()/2));
-		frame.add(splitPane);
-		
-//		
-//		String text = "1,2,3 - Change view mode.\n";
-//		text += "s - Save image.\n";
-//		text += "l - Show trace.\n";
-//		text += "c - Clean map.\n";
-//		JOptionPane.showMessageDialog(null, text);
-	}
-	
-    public void addPoint(Pose p) {
-    		map.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1') map.setVisual(0);
-		else if (input == '2') map.setVisual(1);
-		else if (input == '3') map.setVisual(2);
-		else if (input == 'l') map.showLine();
-		else if (input == 's') map.save();
-		else if (input == 'c') map.clean();
-		
-		 switch(e.getKeyCode()) { 
-	        case KeyEvent.VK_UP:
-	        		sendthread.send(FORWARD);
-	            break;
-	        case KeyEvent.VK_DOWN:
-	        		sendthread.send(BACKWARD);
-	            break;
-	        case KeyEvent.VK_LEFT:
-	        		sendthread.send(LEFT);
-	            break;
-	        case KeyEvent.VK_RIGHT :
-	        		sendthread.send(RIGHT);
-	            break;
-	    }			 
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {		
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-		Robot robot = new VirtualRobot();
-		LineMap map = Map.makeMap();
-		
-		
-//		NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH); 
-//		NXTInfo[] nxtInfo = nxtComm.search("NXT8"); //find brick with NXT_ID by doing a Bluetooth inquiry
-//		if (nxtInfo.length == 0) { // failed to find a brick with the ID
-//			System.err.println("NO NXT found");
-//			System.exit(1);
-//		}
-//		if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-//			System.err.println("Failed to open NXT");
-//			System.exit(1);
-//		}
-//		
-//		final DataInputStream input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-//		final DataOutputStream output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-//		
-		SwingUtilities.invokeLater(new Runnable() {
-            public void run() {
-            	//new Map(input, output);
-
-            	new MainProgram(null, null);
-            }
-        });
-		
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-		
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		
-	}
-
-	@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) {		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/45/60f1f4faabba001711b8bf632416c20d

@@ -0,0 +1,179 @@
+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;
+	
+	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) ;
+		int position = 0;
+		
+		while (run) {
+			if (scann == false)
+				position = 0;
+			
+			if (position == 90 || position == -90)
+				increment *= -1;
+
+			scannerm.rotateTo(position);
+			scannerm.waitComplete();
+
+			if (scann == false)
+				continue;
+			
+			int distance = sensor.getDistance();
+						
+			Pose pose = provider.getPose();
+			float x = pose.getX();
+			float y = pose.getY();
+			float alpha = pose.getHeading();
+			
+			if (distance != 255) {
+				try {
+					output.write('@');
+					output.write(position);
+					output.writeFloat(alpha);
+					output.writeFloat(x);
+					output.write(distance);
+					output.writeFloat(y);
+					output.flush();
+				} catch (IOException e) {
+				}
+			}
+			
+			position += increment;
+		}
+		
+		scannerm.rotateTo(0);
+		scannerm.waitComplete();
+	}
+}
+
+public class Sonar {
+	
+	private static final byte FORWARD = 0;
+	private static final byte STOP = 1;
+	private static final byte EXIT = 2;
+	private static final byte LEFT = 3;
+	private static final byte RIGHT = 4;
+	private static final byte BACKWARD = 5;
+	
+	public static final byte STOPSCANN = 6;
+	public static final byte STARTSCANN = 7;
+	public static final byte MOVE = 8;
+	public static final byte ROTATE = 9;
+	public static final byte SETPOSE = 10;
+	public static final byte SETSCANANGLE = 11;
+	
+	public static void main(String[] args) throws Exception {
+		
+		BTConnection btc = Bluetooth.waitForConnection();
+		//USBConnection btc = USB.waitForConnection();
+		
+		DataInputStream input = btc.openDataInputStream();
+		DataOutputStream output = btc.openDataOutputStream();
+		
+		DifferentialPilot pilot = new DifferentialPilot(5.6f, 13.8f, Motor.B, Motor.C, false); 
+		OdometryPoseProvider provider = new OdometryPoseProvider(pilot);
+		pilot.setRotateSpeed(5);
+		pilot.setTravelSpeed(20);
+		
+		Scanner scan = new Scanner(output, provider);
+		scan.start();
+		int input_byte;
+		boolean run = true;
+		
+		Sound.twoBeeps();
+		
+		while (run) {
+			if (input.available() <= 0) {
+				Thread.yield();
+				continue;
+			}
+			input_byte = input.readByte();
+
+			System.out.println(input_byte);
+			
+			switch (input_byte) {
+				case FORWARD:
+					pilot.forward();
+					break;
+				case STOP:
+					pilot.stop();
+					break;
+				case EXIT:
+					run = false;
+					break;
+				case LEFT:
+					pilot.rotateLeft();
+					break;
+				case RIGHT:
+					pilot.rotateRight();
+					break;
+				case BACKWARD:
+					pilot.backward();
+					break;
+				case STOPSCANN:
+					scan.scann = false;
+					break;
+				case STARTSCANN:
+					scan.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/46/30847c0608b9001711c7a09600ea5d68

@@ -1,113 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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/100.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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		int distance = h*2/25;
-		double d = distance/20.0;
-		double a = -oval_size/4;
-		for (SonarRead r: lista_leituras) {
-			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/47/20310839cab90017124d9f37712eb7c1

@@ -1,225 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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;
-	private Pose refpose;
-
-	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;
-		refpose = new Pose();
-		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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		 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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\n";
-		 text += "<arrows> - Move robot.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 'g')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		else if (input == 's')
-			setRobotPose();
-		
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	private void setRobotPose() {
-		String xs = JOptionPane.showInputDialog("Enter x (m):");
-		String ys = JOptionPane.showInputDialog("Enter y (m):");
-		String as = JOptionPane.showInputDialog("Enter heading (degress):");
-		float x  = Float.parseFloat(xs);
-		float y  = Float.parseFloat(ys);
-		float a  = Float.parseFloat(as);
-		refpose.setHeading(a);
-		refpose.setLocation(x, y);
-	}
-
-	@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();
-		p.setHeading(p.getHeading()-refpose.getHeading());
-		p.setLocation(p.getX()-refpose.getX(), p.getY()-refpose.getY());
-		
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/48/a0279f571db80017145c9b6064c27648

@@ -1,187 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.LinkedList;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-	
-	private LinkedList<DataPose> data;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						data.add(d);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	BluetoothRobot (String name) {
-		this.name = name;
-		data = new LinkedList<DataPose>();
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect () {
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-	}
-
-	@Override
-	public DataPose getData() {
-		return data.pollFirst();
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/49/10d29c82c9b90017124d9f37712eb7c1

@@ -1,206 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		 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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\n";
-		 text += "<arrows> - Move robot.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@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();
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/4a/1097361a10b9001711c7a09600ea5d68

@@ -1,190 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import config.SonarModel;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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 SonarModel 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 SonarModel(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);
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@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.exit();
-
-	}
-
-	@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();
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedRead(p);
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/4a/906561e51cb80017145c9b6064c27648

@@ -1,169 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener {
-
-	private MapImage imap;
-	private Robot robot;
-
-	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) {
-		JFrame frame = new JFrame("Mapa MAC0318");
-
-		
-		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-        frame.setLayout(new BorderLayout());
-        imap = new MapImage(map);
-        ScannerImage scanner = new ScannerImage();
-//		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);
-		
-//		
-//		String text = "1,2,3 - Change view mode.\n";
-//		text += "s - Save image.\n";
-//		text += "l - Show trace.\n";
-//		text += "c - Clean map.\n";
-//		JOptionPane.showMessageDialog(null, text);
-	}
-	
-    public void addPoint(Pose p) {
-    		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1') imap.setVisual(0);
-		else if (input == '2') imap.setVisual(1);
-		else if (input == '3') imap.setVisual(2);
-		else if (input == 'l') imap.showLine();
-		else if (input == 's') imap.save();
-		else if (input == 'c') imap.clean();
-		if (robot == null) return;
-		
-		 switch(e.getKeyCode()) { 
-	        case KeyEvent.VK_UP:
-	        		robot.moveForward();
-	            break;
-	        case KeyEvent.VK_DOWN:
-	        		robot.moveBackward();
-	            break;
-	        case KeyEvent.VK_LEFT:
-	        		robot.moveLeft();
-	            break;
-	        case KeyEvent.VK_RIGHT :
-	        		robot.moveRight();
-	        		sendthread.send(RIGHT);
-	            break;
-	    }			 
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {		
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		
-
-		SwingUtilities.invokeLater(new Runnable() {
-            public void run() {
-            	//new Map(input, output);
-
-            	new MainProgram(map);
-            }
-        });
-		
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-		
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		
-	}
-
-	@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) {		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/4a/f0a9ccc21bb80017145c9b6064c27648

@@ -1,182 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.LinkedList;
-import java.util.List;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-	
-	private List<DataPose> data;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						
-						// posicao do robo
-						imap.addPoint(x, y, (float)Math.toRadians(alpha+90));
-						
-						// ponto do ultrasonico
-						double sensor_ang = Math.toRadians(alpha+angle);
-						double dx = Math.cos(sensor_ang)*distance;
-						double dy = Math.sin(sensor_ang)*distance;
-						imap.addRead(x+dx, y+dy);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	BluetoothRobot (String name) {
-		this.name = name;
-		data = new LinkedList<DataPose>();
-	}
-
-	@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 boolean connect () {
-		try {
-			NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void exit() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public DataPose getData() {
-		// TODO Auto-generated method stub
-		return null;
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/4b/50f3eb5c0db9001711c7a09600ea5d68

@@ -1,185 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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;
-
-	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);
-		// 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);
-		
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-		
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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();
-		imap.addPoint(p);
-		
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle()+p.getHeading());
-		double dx = Math.cos(sensor_ang)*data.getDistance();
-		double dy = Math.sin(sensor_ang)*data.getDistance();
-		imap.addRead(p.getX()+dx, p.getY()+dy);
-		
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle());
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/4b/70ba5cdb19b80017145c9b6064c27648

@@ -1,189 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener {
-
-	private MapImage imap;
-
-	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) {
-		JFrame frame = new JFrame("Mapa MAC0318");
-
-
-		receivethread= new Receiver();
-		sendthread = new Send();
-		
-//		output = out;
-//		input = in;
-
-//		receivethread.start();
-//		sendthread.start();
-		
-		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-        frame.setLayout(new BorderLayout());
-        imap = new MapImage(map);
-        ScannerImage scanner = new ScannerImage();
-//		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);
-		
-//		
-//		String text = "1,2,3 - Change view mode.\n";
-//		text += "s - Save image.\n";
-//		text += "l - Show trace.\n";
-//		text += "c - Clean map.\n";
-//		JOptionPane.showMessageDialog(null, text);
-	}
-	
-    public void addPoint(Pose p) {
-    		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1') imap.setVisual(0);
-		else if (input == '2') imap.setVisual(1);
-		else if (input == '3') imap.setVisual(2);
-		else if (input == 'l') imap.showLine();
-		else if (input == 's') imap.save();
-		else if (input == 'c') imap.clean();
-		
-		 switch(e.getKeyCode()) { 
-	        case KeyEvent.VK_UP:
-	        		sendthread.send(FORWARD);
-	            break;
-	        case KeyEvent.VK_DOWN:
-	        		sendthread.send(BACKWARD);
-	            break;
-	        case KeyEvent.VK_LEFT:
-	        		sendthread.send(LEFT);
-	            break;
-	        case KeyEvent.VK_RIGHT :
-	        		sendthread.send(RIGHT);
-	            break;
-	    }			 
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {		
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		
-		
-//		NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH); 
-//		NXTInfo[] nxtInfo = nxtComm.search("NXT8"); //find brick with NXT_ID by doing a Bluetooth inquiry
-//		if (nxtInfo.length == 0) { // failed to find a brick with the ID
-//			System.err.println("NO NXT found");
-//			System.exit(1);
-//		}
-//		if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-//			System.err.println("Failed to open NXT");
-//			System.exit(1);
-//		}
-//		
-//		final DataInputStream input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-//		final DataOutputStream output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-//		
-		SwingUtilities.invokeLater(new Runnable() {
-            public void run() {
-            	//new Map(input, output);
-
-            	new MainProgram(map);
-            }
-        });
-		
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-		
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		
-	}
-
-	@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) {		
-	}
-}

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

@@ -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.add(p);
+    			lista_pontos.clear();
+	        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/4c/30a3ea86dbb90017124d9f37712eb7c1

@@ -1,224 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	private RobotReturn rr;
-	private Semaphore semaphore;
-	
-	public static final byte FORWARD = 0;
-	public static final byte STOP = 1;
-	public static final byte EXIT = 2;
-	public static final byte LEFT = 3;
-	public static final byte RIGHT = 4;
-	public static final byte BACKWARD = 5;
-	
-	public static final byte STOPSCANN = 6;
-	public static final byte STARTSCANN = 7;
-	public static final byte MOVE = 8;
-	public static final byte ROTATE = 9;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-//	private Send sendthread;
-	private Receiver receivethread;
-		
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						rr.robotData(d);						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-//	private class Send extends Thread {
-//		private int before;
-//		private int command;
-//		private boolean run = true;
-//		
-//		@Override
-//		public void run() {
-//			before = -1;
-//			command = STOP;
-//			
-//			while(run) {
-//				if (before != command) {
-//					try {
-//						System.out.println("Send cmd");
-//						output.write(command);
-//						output.flush();
-//						if (command == EXIT) run = false;
-//					} catch (IOException e1) {
-//						System.out.println("Erro send cmd");
-//						before = -1;
-//						send(command);
-//					}
-//					before = command;
-//				}
-//				
-//				try {
-//					Thread.sleep(50);
-//				} catch (InterruptedException e) {
-//
-//				}
-//			}
-//		}
-//		public void send(int cmd) {
-//			command = cmd;
-//		}
-//	}
-//	
-	
-	public BluetoothRobot (String name) {
-		this.name = name;
-	}
-
-	@Override
-	public void moveForward() {
-//		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-//		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-//		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-//		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect () {
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-
-	@Override
-	public void move(double x) {
-		output.write(position);
-		output.writeFloat(alpha);
-		
-	}
-
-	@Override
-	public void rotate(double x) {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public ArrayList<DataPose> scann(int ini, int end, int interval) {
-		// TODO Auto-generated method stub
-		return null;
-	}
-
-	@Override
-	public void scann(RobotReturn r) {
-		rr = r;
-	}
-
-	@Override
-	public void stopScann() {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void disconnect() {
-		sendthread.send(EXIT);
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-		
-	}
-
-	@Override
-	public void setPose(float x, float y, float a) {
-		// TODO Auto-generated method stub
-		
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/4c/b0baefecaaba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/4d/00a4e69dcab90017124d9f37712eb7c1

@@ -1,222 +0,0 @@
-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 javax.swing.ButtonGroup;
-import javax.swing.JFrame;
-import javax.swing.JMenu;
-import javax.swing.JMenuBar;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JRadioButtonMenuItem;
-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.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;
-		refpose = new Pose();
-		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);
-		
-
-		boolean result = false;
-		if (robot != null)
-			result = robot.connect(this);
-
-		if (result == false) {
-			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
-		}
-
-		 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 global map.\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 += "g - Save global map image.\n";
-		 text += "h - Save scanner image.\n";
-		 text += "<arrows> - Move robot.\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();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 'g')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		else if (input == 's')
-			setRobotPose();
-		
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	private void setRobotPose() {
-		String xs = JOptionPane.showInputDialog("Enter x (m):");
-		String ys = JOptionPane.showInputDialog("Enter y (m):");
-		String as = JOptionPane.showInputDialog("Enter heading (degress):");
-		float x  = Float.parseFloat(xs);
-		float y  = Float.parseFloat(ys);
-		float a  = Float.parseFloat(as);
-	}
-
-	@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();
-		p.setHeading(p.getHeading()-refpose.getHeading());
-		p.setLocation(p.getX()-refpose.getX(), p.getY()-refpose.getY());
-		
-		imap.addPoint(p);
-
-		// ponto do ultrasonico
-		double sensor_ang = Math.toRadians(data.getSensorAngle() + p.getHeading());
-		double dx = Math.cos(sensor_ang) * data.getDistance();
-		double dy = Math.sin(sensor_ang) * data.getDistance();
-		imap.addRead(p.getX() + dx, p.getY() + dy);
-
-		double expected = smodel.expectedSonarRead(p, data.getSensorAngle());
-
-		scanner.addRead(p, data.getDistance(), data.getSensorAngle(), expected);
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		// Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/4e/507eddc519b80017145c9b6064c27648

@@ -1,194 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener {
-
-	private MapImage imap;
-	
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-
-	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) {
-		JFrame frame = new JFrame("Mapa MAC0318");
-
-
-		receivethread= new Receiver();
-		sendthread = new Send();
-		
-//		output = out;
-//		input = in;
-
-//		receivethread.start();
-//		sendthread.start();
-		
-		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-        frame.setLayout(new BorderLayout());
-        imap = new MapImage(map);
-        ScannerImage scanner = new ScannerImage();
-//		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);
-		
-//		
-//		String text = "1,2,3 - Change view mode.\n";
-//		text += "s - Save image.\n";
-//		text += "l - Show trace.\n";
-//		text += "c - Clean map.\n";
-//		JOptionPane.showMessageDialog(null, text);
-	}
-	
-    public void addPoint(Pose p) {
-    		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1') imap.setVisual(0);
-		else if (input == '2') imap.setVisual(1);
-		else if (input == '3') imap.setVisual(2);
-		else if (input == 'l') imap.showLine();
-		else if (input == 's') imap.save();
-		else if (input == 'c') imap.clean();
-		
-		 switch(e.getKeyCode()) { 
-	        case KeyEvent.VK_UP:
-	        		sendthread.send(FORWARD);
-	            break;
-	        case KeyEvent.VK_DOWN:
-	        		sendthread.send(BACKWARD);
-	            break;
-	        case KeyEvent.VK_LEFT:
-	        		sendthread.send(LEFT);
-	            break;
-	        case KeyEvent.VK_RIGHT :
-	        		sendthread.send(RIGHT);
-	            break;
-	    }			 
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {		
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		
-		
-//		NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH); 
-//		NXTInfo[] nxtInfo = nxtComm.search("NXT8"); //find brick with NXT_ID by doing a Bluetooth inquiry
-//		if (nxtInfo.length == 0) { // failed to find a brick with the ID
-//			System.err.println("NO NXT found");
-//			System.exit(1);
-//		}
-//		if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-//			System.err.println("Failed to open NXT");
-//			System.exit(1);
-//		}
-//		
-//		final DataInputStream input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-//		final DataOutputStream output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-//		
-		SwingUtilities.invokeLater(new Runnable() {
-            public void run() {
-            	//new Map(input, output);
-
-            	new MainProgram(map);
-            }
-        });
-		
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-		
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		receivethread.run = false;
-		sendthread.send(EXIT);
-		try {
-			receivethread.join();
-			sendthread.join();
-		} catch (InterruptedException e1) {
-			System.out.println("Nao foi possivel finalizar as threads...");
-		}
-		
-	}
-
-	@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) {		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/4e/70fe90c5aaba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/5/103d474232b80017145c9b6064c27648

@@ -1,113 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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 = distance;
-    			//System.out.println(d);
-    			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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		int distance = h*2/25;
-		double d = distance/20.0;
-		double a = -oval_size/4;
-		for (SonarRead r: lista_leituras) {
-			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/5/104db3ed1eb80017145c9b6064c27648

@@ -1,171 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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;
-
-	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);
-		ScannerImage scanner = new ScannerImage();
-		// 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);
-		
-		boolean result = false;
-		if (robot != null) {
-			result = robot.connect(this);
-		}
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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) {
-		// TODO Auto-generated method stub
-		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/5/60e842061db80017145c9b6064c27648

@@ -1,179 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.LinkedList;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-	
-	private LinkedList<DataPose> data;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						data.add(d);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	BluetoothRobot (String name) {
-		this.name = name;
-		data = new LinkedList<DataPose>();
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect () {
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		try {
-			nxtComm.close();
-		} catch (IOException e) {
-			// TODO Auto-generated catch block
-			e.printStackTrace();
-		}
-	}
-
-	@Override
-	public DataPose getData() {
-		return data.pollFirst();
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/5/c07579811eb80017145c9b6064c27648

@@ -1,170 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.bluetooth.BluetoothConnectionException;
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-import robots.BluetoothRobot;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener, RobotReturn {
-
-	private MapImage imap;
-	private Robot robot;
-
-	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);
-		ScannerImage scanner = new ScannerImage();
-		// 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);
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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) {
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/5/d0c0aac907b9001711c7a09600ea5d68

@@ -1,113 +0,0 @@
-import java.awt.Color;
-import java.awt.Graphics;
-import java.util.ArrayList;
-import java.util.concurrent.Semaphore;
-
-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;
-		SonarRead (double distance, double ang) {
-			this.ang = ang;
-			this.distance = distance;
-		}
-	}
-	
-    @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/250;
-    			
-    			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];
-		            g.drawLine(
-			                (int)(w+(l.x1-pose.getX())*d),
-			                (int)(h-(l.y1-pose.getY())*d), 
-			                (int)(w+(l.x2-pose.getX())*d),
-			                (int)(h-(l.y2-pose.getY())*d)
-			        );
-		    		}
-		    	}
-    		
-    			drawDots(g, w, h);
-    	        semaphore.release();
-    		}
-    	}
-
-	private void drawDots(Graphics g, int w, int h) {
-		g.setColor(new Color(1f, 0f, 0f));
-		int oval_size = 30;
-		int distance = h*2/25;
-		double d = distance/20.0;
-		double a = -oval_size/4;
-		for (SonarRead r: lista_leituras) {
-			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
-			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
-			x = w - x;
-			y = h - y;
-		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 addRead(Pose p, double distance, double ang) {
-		if (semaphore.tryAcquire()) {
-			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
-					&& p.getHeading() == pose.getHeading())) {
-				pose = new Pose(p.getX(), p.getY(), p.getHeading());
-				lista_leituras.clear();
-			}
-			lista_leituras.add(new SonarRead(distance, ang+90));
-	        semaphore.release();
-		}
-		repaint();
-	}
-	
-	public ScannerImage(LineMap map) {
-		this.map = map;
-		semaphore = new Semaphore(1);
-		lista_leituras =  new ArrayList<SonarRead>();
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/50/203f6f78abba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/51/0084fed01eb80017145c9b6064c27648

@@ -1,169 +0,0 @@
-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 javax.swing.JFrame;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-
-import config.Map;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-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;
-
-	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);
-		ScannerImage scanner = new ScannerImage();
-		// 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);
-		
-		if (robot != null)
-			robot.connect(this);
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-		//Robot robot = new BluetoothRobot("NXJ8");
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				new MainProgram(map, robot);
-			}
-		});
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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) {
-		// TODO Auto-generated method stub
-		
-	}
-}

.metadata/.plugins/org.eclipse.core.resources/.history/51/e0c7b99e1cb80017145c9b6064c27648

@@ -1,175 +0,0 @@
-package robots;
-
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-import java.util.LinkedList;
-
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.navigation.Pose;
-
-public class BluetoothRobot implements Robot {
-	private String name;
-	private NXTComm nxtComm;
-	
-	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;
-
-	private DataOutputStream output;
-	private DataInputStream input;
-	private Send sendthread;
-	private Receiver receivethread;
-	
-	private LinkedList<DataPose> data;
-	
-	private class Receiver extends Thread {
-		public boolean run = true;
-		@Override
-		public void run() {
-			int bytes_valiable = -1;
-			
-			while(run) {
-				try {
-					bytes_valiable = input.available();
-				} catch (IOException e1) {
-					// TODO Auto-generated catch block
-					e1.printStackTrace();
-				}
-				if (bytes_valiable >= 0) {
-					try {
-						if (input.readByte() != '@') continue;
-						int angle = input.readByte();
-						float alpha = input.readFloat();
-						float x = input.readFloat();
-						int distance = input.readByte();
-						float y = input.readFloat();
-						DataPose d = new DataPose();
-						d.setDistance(distance);
-						d.setSensorAngle(angle);
-						d.setPose(new Pose(x, y, alpha+90));
-						
-						data.add(d);
-						
-					} catch (IOException e1) {
-						continue;
-					}
-				}
-			}
-		}
-	}
-
-	private class Send extends Thread {
-		private int before;
-		private int command;
-		private boolean run = true;
-		
-		@Override
-		public void run() {
-			before = -1;
-			command = STOP;
-			
-			while(run) {
-				if (before != command) {
-					try {
-						System.out.println("Send cmd");
-						output.write(command);
-						output.flush();
-						if (command == EXIT) run = false;
-					} catch (IOException e1) {
-						System.out.println("Erro send cmd");
-						before = -1;
-						send(command);
-					}
-					before = command;
-				}
-				
-				try {
-					Thread.sleep(50);
-				} catch (InterruptedException e) {
-
-				}
-			}
-		}
-		public void send(int cmd) {
-			command = cmd;
-		}
-	}
-	
-	
-	BluetoothRobot (String name) {
-		this.name = name;
-		data = new LinkedList<DataPose>();
-	}
-
-	@Override
-	public void moveForward() {
-		sendthread.send(FORWARD);
-	}
-
-	@Override
-	public void moveLeft() {
-		sendthread.send(LEFT);
-	}
-
-	@Override
-	public void moveRight() {
-		sendthread.send(RIGHT);
-		
-	}
-
-	@Override
-	public void moveBackward() {
-		sendthread.send(BACKWARD);
-	}
-
-	@Override
-	public boolean connect () {
-		try {
-			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
-			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
-			if (nxtInfo.length == 0) { // failed to find a brick with the ID
-				System.err.println("NO NXT found");
-				return false;
-			}
-			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
-				System.err.println("Failed to open NXT");
-				return false;
-			}
-			
-			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
-			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
-		} catch (NXTCommException e) {
-			return false;
-		}
-		
-		receivethread.start();
-		sendthread.start();
-		
-		return true;	
-	}
-
-	@Override
-	public void stop() {
-		sendthread.send(STOP);
-	}
-
-	@Override
-	public void exit() {
-		sendthread.send(EXIT);
-		nxtComm.close();
-		
-	}
-
-	@Override
-	public DataPose getData() {
-		return data.pollFirst();
-	}
-
-}

.metadata/.plugins/org.eclipse.core.resources/.history/52/305247498fba001711b8bf632416c20d

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

.metadata/.plugins/org.eclipse.core.resources/.history/52/b0dd751f1db80017145c9b6064c27648

@@ -1,169 +0,0 @@
-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.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
-
-import javax.swing.JFrame;
-import javax.swing.JOptionPane;
-import javax.swing.JPanel;
-import javax.swing.JSplitPane;
-import javax.swing.SwingUtilities;
-import javax.swing.event.AncestorEvent;
-import javax.swing.event.AncestorListener;
-
-import config.Map;
-import lejos.pc.comm.NXTComm;
-import lejos.pc.comm.NXTCommException;
-import lejos.pc.comm.NXTCommFactory;
-import lejos.pc.comm.NXTInfo;
-import lejos.robotics.mapping.LineMap;
-import lejos.robotics.navigation.Pose;
-import robots.Robot;
-import robots.VirtualRobot;
-
-public class MainProgram extends JPanel implements KeyListener, WindowListener {
-
-	private MapImage imap;
-	private Robot robot;
-
-	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) {
-		JFrame frame = new JFrame("Mapa MAC0318");
-
-		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
-		frame.setLayout(new BorderLayout());
-		imap = new MapImage(map);
-		ScannerImage scanner = new ScannerImage();
-		// 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);
-
-		//
-		// String text = "1,2,3 - Change view mode.\n";
-		// text += "s - Save image.\n";
-		// text += "l - Show trace.\n";
-		// text += "c - Clean map.\n";
-		// JOptionPane.showMessageDialog(null, text);
-	}
-
-	public void addPoint(Pose p) {
-		imap.addPoint(p);
-	}
-
-	@Override
-	public void keyPressed(KeyEvent e) {
-		char input = e.getKeyChar();
-		if (input == '1')
-			imap.setVisual(0);
-		else if (input == '2')
-			imap.setVisual(1);
-		else if (input == '3')
-			imap.setVisual(2);
-		else if (input == 'l')
-			imap.showLine();
-		else if (input == 's')
-			imap.save();
-		else if (input == 'c')
-			imap.clean();
-		if (robot == null)
-			return;
-
-		switch (e.getKeyCode()) {
-		case KeyEvent.VK_UP:
-			robot.moveForward();
-			break;
-		case KeyEvent.VK_DOWN:
-			robot.moveBackward();
-			break;
-		case KeyEvent.VK_LEFT:
-			robot.moveLeft();
-			break;
-		case KeyEvent.VK_RIGHT:
-			robot.moveRight();
-			break;
-		}
-	}
-
-	@Override
-	public void keyReleased(KeyEvent arg0) {
-		if (robot == null)
-			return;
-		robot.stop();
-	}
-
-	@Override
-	public void keyTyped(KeyEvent arg0) {
-	}
-
-	public static void main(String[] args) throws NXTCommException, IOException {
-
-		LineMap map = Map.makeMap();
-		Robot robot = new VirtualRobot(map);
-
-		SwingUtilities.invokeLater(new Runnable() {
-			public void run() {
-				// new Map(input, output);
-
-				new MainProgram(map);
-			}
-		});
-
-	}
-
-	@Override
-	public void windowOpened(WindowEvent e) {
-
-	}
-
-	@Override
-	public void windowClosing(WindowEvent e) {
-		System.err.println("Fechando...");
-		if (robot == null)
-			return;
-		robot.exit();
-
-	}
-
-	@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) {
-	}
-}