Merge branch 'develop'

.settings/org.eclipse.jdt.core.prefs

@@ -0,0 +1,4 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6
+org.eclipse.jdt.core.compiler.compliance=1.6
+org.eclipse.jdt.core.compiler.source=1.6

AndroidManifest.xml

@@ -17,8 +17,8 @@
 
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
     package="ve.ucv.ciens.ccg.nxtcam"
-    android:versionCode="1"
-    android:versionName="1.0" >
+    android:versionCode="140626"
+    android:versionName="14.06.26" >
 
     <uses-sdk
         android:minSdkVersion="11"

README.md

@@ -1,4 +1,4 @@
-NxtCAM
-======
+NxtAR-cam
+=========
 
 Modulo 1 de mi trabajo especial de grado.

src/ve/ucv/ciens/ccg/networkdata/MotorEvent.java

@@ -0,0 +1,37 @@
+package ve.ucv.ciens.ccg.networkdata;
+
+import java.io.Serializable;
+
+public class MotorEvent implements Serializable{
+	private static final long serialVersionUID = 9989L;
+
+	public enum motor_t {NONE, MOTOR_A, MOTOR_B, MOTOR_C, MOTOR_AC, RECENTER, ROTATE_90};
+
+	private motor_t motor;
+	private byte power;
+
+	public MotorEvent(){
+		motor = motor_t.NONE;
+		power = 0;
+	}
+
+	public void setMotor(motor_t motor){
+		this.motor = motor;
+	}
+
+	public void setPower(byte power) throws IllegalArgumentException{
+		if(power > 100 || power < -100){
+			throw new IllegalArgumentException("Motor power must be a number between -100 and 100");
+		}else{
+			this.power = power;
+		}
+	}
+
+	public motor_t getMotor(){
+		return this.motor;
+	}
+
+	public byte getPower(){
+		return this.power;
+	}
+}

src/ve/ucv/ciens/ccg/networkdata/MotorEventACK.java

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2013 Miguel Angel Astor Romero
+ * Copyright (C) 2014 Miguel Angel Astor Romero
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -13,15 +13,20 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-package ve.ucv.ciens.ccg.nxtcam.network;
+package ve.ucv.ciens.ccg.networkdata;
 
-public class LCPThread extends Thread{
+import java.io.Serializable;
 
-	public LCPThread(){
+public class MotorEventACK implements Serializable {
+	private static final long serialVersionUID = 9989L;
 
+	private boolean clientQueueIsFull;
+
+	public MotorEventACK(boolean isQueueFull){
+		this.clientQueueIsFull = isQueueFull;
 	}
 
-	public void run(){
-		
+	public boolean isClientQueueFull(){
+		return this.clientQueueIsFull;
 	}
 }

src/ve/ucv/ciens/ccg/networkdata/SensorDataMessage.java

@@ -0,0 +1,8 @@
+package ve.ucv.ciens.ccg.networkdata;
+
+import java.io.Serializable;
+
+public class SensorDataMessage implements Serializable{
+	private static final long serialVersionUID = 9989L;
+
+}

src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java

@@ -16,8 +16,9 @@
 package ve.ucv.ciens.ccg.nxtcam;
 
 import ve.ucv.ciens.ccg.nxtcam.camera.CameraPreview;
+import ve.ucv.ciens.ccg.nxtcam.network.NxtBTCommThread;
+import ve.ucv.ciens.ccg.nxtcam.network.SensorReportThread;
 import ve.ucv.ciens.ccg.nxtcam.network.VideoStreamingThread;
-import ve.ucv.ciens.ccg.nxtcam.network.LCPThread;
 import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
 import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants;
 import android.app.Activity;
@@ -40,7 +41,8 @@ public class CamActivity extends Activity{
 	private CameraPreview cPreview;
 	private CameraSetupTask camSetupTask;
 	private VideoStreamingThread imThread;
-	private LCPThread botThread;
+	private NxtBTCommThread botThread;
+	private SensorReportThread sensorThread;
 	private String serverIp;
 
 	/*******************
@@ -56,6 +58,12 @@ public class CamActivity extends Activity{
 		serverIp = intent.getStringExtra("address");
 		imThread = new VideoStreamingThread(serverIp);
 		imThread.start();
+
+		botThread = new NxtBTCommThread(serverIp);
+		botThread.start();
+
+		sensorThread = new SensorReportThread(serverIp);
+		sensorThread.start();
 	}
 
 	@Override
@@ -89,14 +97,14 @@ public class CamActivity extends Activity{
 		camSetupTask = new CameraSetupTask();
 		camSetupTask.execute();
 
-		// imThread.start();
+		// TODO: resumethe imThread, botThread and sensorThread objects.
 	}
 
 	@Override
 	public void onPause(){
 		super.onPause();
 
-		// TODO: pause the imThread and botThread objects.
+		// TODO: pause the imThread, botThread and sensorThread objects.
 
 		if(cPreview != null){
 			cPreview.removePreviewCallback();
@@ -108,8 +116,14 @@ public class CamActivity extends Activity{
 	@Override
 	public void onDestroy(){
 		super.onDestroy();
-		// TODO: Destroy the network threads.
+		imThread.finish();
 		imThread = null;
+
+		botThread.finish();
+		botThread = null;
+
+		sensorThread.finish();
+		sensorThread = null;
 	}
 
 	@Override

src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java

@@ -19,7 +19,6 @@ import java.io.IOException;
 import java.net.DatagramPacket;
 import java.net.InetAddress;
 import java.net.MulticastSocket;
-import java.net.Socket;
 
 import ve.ucv.ciens.ccg.nxtcam.dialogs.ConnectRobotDialog;
 import ve.ucv.ciens.ccg.nxtcam.dialogs.ConnectRobotDialog.ConnectRobotDialogListener;
@@ -324,7 +323,7 @@ public class MainActivity extends Activity implements WifiOnDialogListener, Conn
 		public ServiceDiscoveryTask(){
 			// Open a multicast socket and join the project's multicast group.
 			try{
-				udpSocket = new MulticastSocket(ProjectConstants.SERVER_UDP_PORT);
+				udpSocket = new MulticastSocket(ProjectConstants.SERVICE_DISCOVERY_PORT);
 				InetAddress group = InetAddress.getByName(ProjectConstants.MULTICAST_ADDRESS);
 				udpSocket.joinGroup(group);
 			}catch(IOException io){

src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java

@@ -30,6 +30,8 @@ import android.annotation.SuppressLint;
 import android.app.Activity;
 import android.content.Context;
 import android.hardware.Camera;
+import android.hardware.Camera.CameraInfo;
+import android.hardware.Camera.Parameters;
 import android.hardware.Camera.Size;
 import android.os.Build;
 import android.view.Surface;
@@ -39,22 +41,23 @@ import android.view.SurfaceView;
 /** A basic Camera preview class */
 @SuppressLint("ViewConstructor")
 public class CameraPreview extends SurfaceView implements SurfaceHolder.Callback, Camera.PreviewCallback {
-	private final String TAG = "SURFVIEW";
-	private final String CLASS_NAME = CameraPreview.class.getSimpleName();
+	private static final String TAG            = "SURFVIEW";
+	private static final String CLASS_NAME     = CameraPreview.class.getSimpleName();
+	private static final int    OPTIMAL_WIDTH  = 352;
+	private static final int    OPTIMAL_HEIGHT = 288;
 
 	private CameraImageMonitor imgMonitor;
-	private Activity parentActivity;
-	private SurfaceHolder holder;
-	private Camera camera;
-	private int previewWidth;
-	private int previewHeight;
+	private Activity           parentActivity;
+	private SurfaceHolder      holder;
+	private Camera             camera;
+	private int                previewWidth;
+	private int                previewHeight;
 
 	@SuppressWarnings("deprecation")
 	public CameraPreview(Context context, Camera camera){
 		super(context);
 		parentActivity = (Activity)context;
 
-		// surfaceView = new SurfaceView(context);
 		holder = getHolder();
 		holder.addCallback(this);
 
@@ -62,78 +65,99 @@ public class CameraPreview extends SurfaceView implements SurfaceHolder.Callback
 			holder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
 	}
 
+	/**
+	 * <p>Sets the camera for this surface view.</p>
+	 * 
+	 * @param camera The camera to set.
+	 */
 	public void setCamera(Camera camera){
 		this.camera = camera;
 		if(this.camera != null){
-			Logger.log_d(TAG, CLASS_NAME + ".setCamera() :: Setting camera.");
+			Logger.log_d(TAG, CLASS_NAME + ".setCamera(): Setting camera.");
 			imgMonitor = CameraImageMonitor.getInstance();
 			requestLayout();
-			Logger.log_d(TAG, CLASS_NAME + ".setCamera() :: Camera set.");
+			Logger.log_d(TAG, CLASS_NAME + ".setCamera(): Camera set.");
 		}
 	}
 
+	@Override
 	public void surfaceCreated(SurfaceHolder holder){
 		// The Surface has been created, now tell the camera where to draw the preview.
-		Logger.log_d(TAG, CLASS_NAME + ".surfaceCreated() :: Creating surface view.");
+		Logger.log_d(TAG, CLASS_NAME + ".surfaceCreated(): Creating surface view.");
 		try {
 			if(camera != null)
 				camera.setPreviewDisplay(holder);
 		} catch (IOException e) {
-			Logger.log_e(TAG, CLASS_NAME + ".surfaceCreated() :: Error creating camera: " + e.getMessage());
+			Logger.log_e(TAG, CLASS_NAME + ".surfaceCreated(): Error creating camera: " + e.getMessage());
 		}
 	}
 
+	@Override
 	public void surfaceDestroyed(SurfaceHolder holder){
 		if(camera != null)
 			camera.stopPreview();
 	}
 
+	@Override
 	public void surfaceChanged(SurfaceHolder tmpHolder, int format, int w, int h){
-		int result;
-		int rotation;
-		int degrees = 0;
-		Camera.Parameters camParams;
+		int        result;
+		int        rotation;
+		int        degrees = 0;
+		Size       optimal;
+		CameraInfo info;
+		List<Size> sizes;
+		Parameters camParams;
 
-		Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged() :: Method started.");
+		Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged(): Method started.");
 		if(this.holder.getSurface() == null || camera == null){
-			Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged() :: Holder and/or camera are null.");
+			Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged(): Holder and/or camera are null.");
 			return;
 		}
 
 		try{ camera.stopPreview(); }catch (Exception e){ }
 
+		Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged(): Request layout.");
 		requestLayout();
 
+		Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged(): Get parameters.");
 		camParams = camera.getParameters();
-		camParams.getSupportedPreviewSizes();
-		List<Size> sizes = camParams.getSupportedPreviewSizes();
+		sizes = camParams.getSupportedPreviewSizes();
 
-		/*for(Size size: sizes){
-			Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged() :: Supported preview size (" + size.width + ", " + size.height + ")");
+		for(Size size: sizes){
+			Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged(): Supported preview size (" + size.width + ", " + size.height + ")");
 		}
 
-		Size optimal = getOptimalPreviewSize(sizes, w, h);
-		Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged() :: Preview size set at (" + optimal.width + ", " + optimal.height + ")");
-		camParams.setPreviewSize(optimal.width, optimal.height);*/
-		camParams.setPreviewSize(720, 480);
+		optimal = getOptimalPreviewSize(sizes, camera.new Size(OPTIMAL_WIDTH, OPTIMAL_HEIGHT));
+		Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged(): Preview size set at (" + optimal.width + ", " + optimal.height + ")");
+		camParams.setPreviewSize(optimal.width, optimal.height);
 		camera.setParameters(camParams);
-		/*previewWidth = optimal.width;
-		previewHeight = optimal.height;*/
-		previewWidth = 720;
-		previewHeight = 480;
+		previewWidth = optimal.width;
+		previewHeight = optimal.height;
 
-		android.hardware.Camera.CameraInfo info = new android.hardware.Camera.CameraInfo();
-		android.hardware.Camera.getCameraInfo(0, info);
+		info = new CameraInfo();
+		Camera.getCameraInfo(0, info);
 		rotation = parentActivity.getWindowManager().getDefaultDisplay().getRotation();
 
 		switch (rotation) {
-		case Surface.ROTATION_0: degrees = 0; break;
-		case Surface.ROTATION_90: degrees = 90; break;
-		case Surface.ROTATION_180: degrees = 180; break;
-		case Surface.ROTATION_270: degrees = 270; break;
+		default:
+		case Surface.ROTATION_0:
+			degrees = 0;
+			break;
+
+		case Surface.ROTATION_90:
+			degrees = 90;
+			break;
+
+		case Surface.ROTATION_180:
+			degrees = 180;
+			break;
+
+		case Surface.ROTATION_270:
+			degrees = 270;
+			break;
 		}
 
-		if (info.facing == Camera.CameraInfo.CAMERA_FACING_FRONT) {
+		if (info.facing == CameraInfo.CAMERA_FACING_FRONT) {
 			result = (info.orientation + degrees) % 360;
 			result = (360 - result) % 360;  // compensate the mirror
 		} else {  // back-facing
@@ -146,7 +170,7 @@ public class CameraPreview extends SurfaceView implements SurfaceHolder.Callback
 			camera.setPreviewDisplay(this.holder);
 			camera.startPreview();
 		}catch (Exception e){
-			Logger.log_e(TAG, CLASS_NAME + ".surfaceChanged() :: Error starting camera preview: " + e.getMessage());
+			Logger.log_e(TAG, CLASS_NAME + ".surfaceChanged(): Error starting camera preview: " + e.getMessage());
 		}
 	}
 
@@ -154,51 +178,47 @@ public class CameraPreview extends SurfaceView implements SurfaceHolder.Callback
 	public void onPreviewFrame(byte[] data, Camera camera){
 		if(imgMonitor.hasChanged())
 			imgMonitor.setImageParameters(previewWidth, previewHeight);
-		Logger.log_d(TAG, CLASS_NAME + ".onPreviewFrame() :: Preview received");
-		Logger.log_d(TAG, CLASS_NAME + ".onPreviewFrame() :: Frame has" + (imgMonitor.hasChanged() ? "" : " not") + " been consumed.");
 		imgMonitor.setImageData(data);
 	}
 
+	/**
+	 * <p>If camera is not null then disable the camera preview.</p>
+	 */
 	public void removePreviewCallback(){
 		if(camera != null)
 			camera.setPreviewCallback(null);
 	}
 
-	private Size getOptimalPreviewSize(List<Size> sizes, int w, int h) {
-		final double ASPECT_TOLERANCE = 0.1;
-		double targetRatio = (double) w / h;
+	/**
+	 * <p>Tries to obtain the preview size that better matches the target size pixel dimensions.</p>
+	 * 
+	 * @param sizes An initialized list of supported preview sizes.
+	 * @param target The ideal preview size.
+	 * @return The size from the list of sizes that better matches the target. Null if sizes is null or empty.
+	 */
+	private Size getOptimalPreviewSize(List<Size> sizes, final Size target) {
+		final int    TARGET_HEIGHT    = target.height;
+		Size         optimalSize      = null;
+		double       minDiff          = Double.MAX_VALUE;
 
-		Logger.log_d(TAG, CLASS_NAME + ".getOptimalPreviewSize() :: Method started.");
-		if (sizes == null) return null;
-
-		Size optimalSize = null;
-		double minDiff = Double.MAX_VALUE;
-
-		int targetHeight = h;
-
-		// Try to find an size match aspect ratio and size
-		for (Size size : sizes) {
-			double ratio = (double) size.width / size.height;
-			if (Math.abs(ratio - targetRatio) > ASPECT_TOLERANCE) continue;
-			if (Math.abs(size.height - targetHeight) < minDiff) {
-				optimalSize = size;
-				minDiff = Math.abs(size.height - targetHeight);
-			}
-		}
+		if (sizes == null || sizes.size() == 0) return null;
 
 		// Cannot find the one match the aspect ratio, ignore the requirement
 		if (optimalSize == null) {
+			Logger.log_d(TAG, CLASS_NAME + ".getOptimalPreviewSize(): Ignoring aspect ratio.");
 			minDiff = Double.MAX_VALUE;
 			for (Size size : sizes) {
-				if (Math.abs(size.height - targetHeight) < minDiff) {
+				if (Math.abs(size.height - TARGET_HEIGHT) < minDiff) {
 					optimalSize = size;
-					minDiff = Math.abs(size.height - targetHeight);
+					minDiff = Math.abs(size.height - TARGET_HEIGHT);
+					Logger.log_d(TAG, CLASS_NAME + ".getOptimalPreviewSize(): Size = (" + Integer.toString(size.width) + ", " + Integer.toString(size.width) + ")");
+					Logger.log_d(TAG, CLASS_NAME + ".getOptimalPreviewSize(): minDiff = " + Double.toString(minDiff));
 				}
 			}
 		}
-		Logger.log_d(TAG, CLASS_NAME + ".getOptimalPreviewSize() :: Method ended.");
-		Logger.log_d(TAG, CLASS_NAME + ".getOptimalPreviewSize() :: Optimal size is: (" + Integer.toString(optimalSize.width) +
-				", " + Integer.toString(optimalSize.height) + ")");
+
+		Logger.log_d(TAG, CLASS_NAME + ".getOptimalPreviewSize(): Optimal size found is (" + Integer.toString(optimalSize.width) + ", " + Integer.toString(optimalSize.height) + ")");
+
 		return optimalSize;
 	}
 }

src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java

@@ -228,9 +228,6 @@ public class BTCommunicator{
 		if(connected){
 			try{
 				byte[] message = new byte[bytes];
-				for(int i = 0; i < message.length; ++i){
-					message[i] = 0x00;
-				}
 				nxtInputStream.read(message, 0, bytes);
 				return message;
 			}catch(IOException e){

src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java

@@ -1,206 +0,0 @@
-/*
- * Copyright (C) 2013 Miguel Angel Astor Romero
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-package ve.ucv.ciens.ccg.nxtcam.network;
-
-import java.io.IOException;
-import java.io.InputStream;
-import java.io.OutputStream;
-import java.util.Set;
-import java.util.UUID;
-
-import android.bluetooth.BluetoothAdapter;
-import android.bluetooth.BluetoothDevice;
-import android.bluetooth.BluetoothSocket;
-import android.util.Log;
-
-public class BluetoothManager{
-	private static final UUID SERIAL_PORT_SERVICE_CLASS_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
-	private static final String OUI_LEGO = "00:16:53";
-	private static final String TAG = "BTMNGR";
-	
-	private boolean connected;
-	private BluetoothAdapter bt_adapter;
-	private BluetoothSocket bt_socket = null;
-	private OutputStream nxt_out_stream = null;
-	private InputStream nxt_in_stream = null;
-
-	private static class SingletonHolder{
-		public static final BluetoothManager INSTANCE = new BluetoothManager();
-	}
-
-	private BluetoothManager(){
-		connected = false;
-		bt_adapter = BluetoothAdapter.getDefaultAdapter();
-		bt_socket = null;
-		nxt_in_stream = null;
-		nxt_out_stream = null;
-	}
-	
-	public static BluetoothManager getInstance(){
-		return SingletonHolder.INSTANCE;
-	}
-
-	public boolean isBTSupported(){
-		return bt_adapter != null;
-	}
-
-	public boolean isConnected(){
-		return connected;
-	}
-
-	public boolean isBTEnabled(){
-		return bt_adapter.isEnabled();
-	}
-
-	public void disableBT(){
-		bt_adapter.disable();
-	}
-
-	public Set<BluetoothDevice> getPairedDevices(){
-		return bt_adapter.getBondedDevices();
-	}
-
-	/**
-	 * Sets up a connection with a NXT device.
-	 * 
-	 * Verifies if the target device is a valid NXT robot by checking agains Lego's OUI.
-	 * Also creates the socket and the streams associated with the connection
-	 * 
-	 * @param mac_address	The mac address of the target device.
-	 * @return true if the connection was established succesfully, otherwise false.
-	 * @throws IOException
-	 */
-	public boolean establishConnection(String mac_address) throws IOException{
-		if (!bt_adapter.isEnabled()){
-			return false;
-		}
-		if(connected){
-			return false;
-		}
-		if(bt_adapter.isEnabled()){
-			if(mac_address == "NONE"){
-				return false;
-			}else{
-				if(mac_address.substring(0, 8).compareTo(OUI_LEGO) != 0){
-					Log.d(TAG, "establishConnection() :: Not a Lego MAC. Prefix : " + mac_address.substring(0, 8) + " :: OUI : " + OUI_LEGO);
-					return false;
-				}else{
-					try{
-						Log.d(TAG, "establishConnection() :: Getting device with mac address: " + mac_address);
-						BluetoothDevice nxtDevice = null;
-						nxtDevice = bt_adapter.getRemoteDevice(mac_address);
-						if (nxtDevice == null) {
-							Log.e(TAG, "establishConnection() :: No device found.");
-							throw new IOException();
-						}
-
-						Log.d(TAG, "establishConnection() :: Opening socket.");
-						bt_socket = nxtDevice.createRfcommSocketToServiceRecord(SERIAL_PORT_SERVICE_CLASS_UUID);
-						Log.d(TAG, "establishConnection() :: Connecting.");
-						bt_socket.connect();
-
-						Log.d(TAG, "establishConnection() :: Opening IO streams.");
-						nxt_in_stream = bt_socket.getInputStream();
-						nxt_out_stream = bt_socket.getOutputStream();
-
-						Log.d(TAG, "establishConnection() :: Connection established.");
-						connected = true;
-
-					}catch(IOException e){
-						Log.e(TAG, "establishConnection() :: Connection failed.");
-						Log.e(TAG, Log.getStackTraceString(e));
-						connected = false;
-						throw e;
-					}
-					return connected;
-				}
-			}
-		}
-		return false;
-	}
-
-	/**
-	 * Closes the active connection if any.
-	 * 
-	 * Additionally clears the socket and the streams associated to said connection.
-	 * 
-	 * @return true if the connection was succesfully closed; false if no connection exists.
-	 * @throws IOException
-	 */
-	public boolean stopConnection() throws IOException{
-		try{
-			if(bt_socket != null){
-				Log.d(TAG, "stopConnection() :: Closing connection.");
-				bt_socket.close();
-				bt_socket = null;
-				nxt_in_stream = null;
-				nxt_out_stream = null;
-				connected = false;
-				Log.d(TAG, "stopConnection() :: Connection closed.");
-				return true;
-			}
-		}catch( IOException e){
-			Log.e(TAG, "stopConnection()");
-			Log.e(TAG, Log.getStackTraceString(e));
-			throw e;
-		}
-		return false;
-	}
-
-	/**
-	 * Sends a message to the NXT robot.
-	 * 
-	 * @param message	The data to be sent.
-	 * @throws IOException
-	 */
-	public synchronized void writeMessage(byte[] message) throws IOException{
-		if(connected){
-			try{
-				nxt_out_stream.write(message);
-				nxt_out_stream.flush();
-			}catch(IOException e){
-				Log.e(TAG, "writeMessage()");
-				Log.e(TAG, Log.getStackTraceString(e));
-				throw e;
-			}
-		}
-	}
-
-	/**
-	 * Reads a message sent by the NXT robot.
-	 * 
-	 * @return The data received as a byte[] if a valid connection exists, otherwise null. 
-	 * @throws IOException
-	 */
-	public synchronized byte[] readMessage(int bytes) throws IOException{
-		if(connected){
-			try{
-				byte[] message = new byte[bytes];
-				for(int i = 0; i < message.length; ++i){
-					message[i] = 0x00;
-				}
-				nxt_in_stream.read(message, 0, bytes);
-				return message;
-			}catch(IOException e){
-				Log.e(TAG, "readMessage()");
-				Log.e(TAG, Log.getStackTraceString(e));
-				throw e;
-			}
-		}else{
-			return null;
-		}
-	}
-}

src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java

@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2014 Miguel Angel Astor Romero
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package ve.ucv.ciens.ccg.nxtcam.network;
+
+import java.io.IOException;
+import java.io.ObjectInputStream;
+import java.io.ObjectOutputStream;
+import java.net.Socket;
+
+import ve.ucv.ciens.ccg.networkdata.MotorEvent;
+import ve.ucv.ciens.ccg.networkdata.MotorEventACK;
+import ve.ucv.ciens.ccg.nxtcam.robotcontrol.MotorEventQueue;
+import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
+import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants;
+
+public class MotorControlThread extends Thread {
+	private static final String TAG = "MOTOR_CONTROL";
+	private static final String CLASS_NAME = MotorControlThread.class.getSimpleName();
+
+	private Socket socket;
+	private String serverIp;
+	private MotorEventQueue queue;
+	private boolean done;
+	private ObjectInputStream reader;
+	private ObjectOutputStream writer;
+	private boolean connected;
+
+	public MotorControlThread(String serverIp){
+		super("Motor Control Thread");
+		this.serverIp = serverIp;
+		done = false;
+		connected = false;
+		queue = MotorEventQueue.getInstance();
+	}
+
+	@Override
+	public void run(){
+		Object msg;
+		MotorEvent event;
+		MotorEventACK ack;
+
+		if(!connectToServer()){
+			Logger.log_e(TAG, CLASS_NAME + ".run() :: The thread is not connected to a server. Finishing.");
+			return;
+		}else{
+			while(!done){
+				// Receive a message and enqueue it;
+				msg = readMessage();
+				event = verifyMessage(msg);
+				if(event != null){
+					queue.addEvent(event);
+					Logger.log_i(TAG, CLASS_NAME + ".run() :: Motor control message enqueued.");
+					Logger.log_i(TAG, CLASS_NAME + ".run() :: Motor ID: " + event.getMotor().toString());
+					Logger.log_i(TAG, CLASS_NAME + ".run() :: Motor power: " + Byte.toString(event.getPower()));
+				}else{
+					Logger.log_i(TAG, CLASS_NAME + ".run() :: Message could not be verified;");
+				}
+
+				// Send corresponding ack;
+				ack = new MotorEventACK(queue.getSize() >= 10);
+				try{
+					writer.writeObject(ack);
+					Logger.log_i(TAG, CLASS_NAME + ".run() :: First ACK sent.");
+				}catch(Exception ex){
+					Logger.log_e(TAG, CLASS_NAME + ".run() :: Exception while sending first ACK: " + ex.getMessage());
+					break;
+				}
+
+				if(ack.isClientQueueFull()){
+					while(queue.getSize() >= 10){ }
+
+					ack = new MotorEventACK(false);
+
+					try{
+						writer.writeObject(ack);
+					}catch(Exception ex){
+						Logger.log_i(TAG, CLASS_NAME + ".run() :: Second ACK sent.");
+						Logger.log_e(TAG, CLASS_NAME + ".run() :: Exception while sending second ACK: " + ex.getMessage());
+						break;
+					}
+				}
+
+				event = null;
+				ack = null;
+				msg = null;
+			}
+			try{
+				socket.close();
+			}catch(IOException io){
+				Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException while closing socket: " + io.getMessage());
+			}
+		}
+	}
+
+	public void finish(){
+		done = true;
+	}
+
+	public boolean connectToServer(){
+		try{
+			socket = new Socket(serverIp, ProjectConstants.MOTOR_CONTROL_PORT);
+			reader = new ObjectInputStream(socket.getInputStream());
+			writer = new ObjectOutputStream(socket.getOutputStream());
+			connected = true;
+		}catch(IOException io){
+			Logger.log_e(TAG, CLASS_NAME + ".connectToServer() :: IOException caught: " + io.getMessage());
+			connected = false;
+		}
+		return connected;
+	}
+
+	private Object readMessage(){
+		Object message;
+		try{
+			message = reader.readObject();
+			Logger.log_i(TAG, CLASS_NAME + ".readMessage() :: Motor control message received.");
+		}catch(ClassNotFoundException cn){
+			Logger.log_e(TAG, CLASS_NAME + ".readMessage() :: ClassNotFoundException caught: " + cn.getMessage());
+			message = null;
+		}catch(IOException io){
+			Logger.log_e(TAG, CLASS_NAME + ".readMessage() :: IOException caught: " + io.getMessage());
+			message = null;
+		}
+		return message;
+	}
+
+	private MotorEvent verifyMessage(Object message){
+		if(message != null && message instanceof MotorEvent){
+			Logger.log_i(TAG, CLASS_NAME + ".verifyMessage() :: Valid motor control message received.");
+			return (MotorEvent)message;
+		}else{
+			return null;
+		}
+	}
+	
+	public boolean isConnected(){
+		return connected;
+	}
+}

src/ve/ucv/ciens/ccg/nxtcam/network/NxtBTCommThread.java

@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2013 Miguel Angel Astor Romero
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package ve.ucv.ciens.ccg.nxtcam.network;
+
+import java.io.IOException;
+
+import ve.ucv.ciens.ccg.networkdata.MotorEvent;
+import ve.ucv.ciens.ccg.networkdata.MotorEvent.motor_t;
+import ve.ucv.ciens.ccg.nxtcam.network.protocols.MotorMasks;
+import ve.ucv.ciens.ccg.nxtcam.robotcontrol.MotorEventQueue;
+import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
+
+public class NxtBTCommThread extends Thread{
+	private static final String TAG = "LCP_THREAD";
+	private static final String CLASS_NAME = NxtBTCommThread.class.getSimpleName();
+
+	private boolean done;
+	private BTCommunicator btComm;
+	private MotorControlThread motorControl;
+
+	private MotorEventQueue queue;
+
+	public NxtBTCommThread(String serverIp){
+		super("Robot Control Main Thread");
+		btComm = BTCommunicator.getInstance();
+		done = false;
+		motorControl = new MotorControlThread(serverIp);
+		queue = MotorEventQueue.getInstance();
+	}
+
+	public void run(){
+		long then, now, delta;
+		MotorEvent event;
+		byte[] msg = new byte[2];
+
+		motorControl.start();
+
+		then = System.currentTimeMillis();
+
+		while(!motorControl.isConnected()){
+			now = System.currentTimeMillis();
+			delta = now - then;
+			if(delta > 9000L){
+				Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server.");
+				return;
+			}
+		}
+
+		while(!done){
+			if(btComm.isBTEnabled() && btComm.isConnected()){
+				msg[0] = 0x00;
+				msg[1] = 0x00;
+
+				event = queue.getNextEvent();
+
+				try{
+					// Set the motor bit.
+					msg[0] |= (event.getMotor() == motor_t.MOTOR_A) ? MotorMasks.MOTOR_A : 0;
+					msg[0] |= (event.getMotor() == motor_t.MOTOR_B) ? MotorMasks.MOTOR_B : 0;
+					msg[0] |= (event.getMotor() == motor_t.MOTOR_C) ? MotorMasks.MOTOR_C : 0;
+					// Set the direction bit.
+					if(event.getPower() > 0) msg[0] |= MotorMasks.DIRECTION;
+					// Set the recenter bit.
+					msg[0] |= (event.getMotor() == motor_t.RECENTER) ? MotorMasks.RECENTER : 0;
+					if((msg[0] & MotorMasks.RECENTER) > 0)
+						Logger.log_i(TAG, CLASS_NAME + ".run(): Recenter received."); 
+
+					// Set the rotate bit.
+					msg[0] |= (event.getMotor() == motor_t.ROTATE_90) ? MotorMasks.ROTATE_90 : 0;
+					if((msg[0] & MotorMasks.ROTATE_90) > 0)
+						Logger.log_i(TAG, CLASS_NAME + ".run(): Rotate 90 received."); 
+
+					// Set the power byte.
+					msg[1] = (byte)Math.abs(event.getPower());
+
+					// Send the message.
+					btComm.writeMessage(msg);
+					Logger.log_i(TAG, CLASS_NAME + ".run() :: Message sent to the robot.");
+
+					try{ sleep(40); }catch(InterruptedException ie){ }
+
+				}catch(IOException io){
+					Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException sending message to the robot: " + io.getMessage());
+				}
+			}else{
+				Logger.log_e(TAG, CLASS_NAME +  ".run() :: The robot disconnected or was never available.");
+				break;
+			}
+		}
+	}
+
+	public void finish(){
+		done = true;
+	}
+}

src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java

@@ -0,0 +1,73 @@
+package ve.ucv.ciens.ccg.nxtcam.network;
+
+import java.io.IOException;
+import java.io.OutputStream;
+import java.net.Socket;
+
+import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
+import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants;
+
+public class SensorReportThread extends Thread{
+	private static final String TAG = "SENSOR_REPORT";
+	private static final String CLASS_NAME = SensorReportThread.class.getSimpleName();
+
+	private Socket socket;
+	private String serverIp;
+	private boolean done;
+	private OutputStream writer;
+	private boolean connected;
+	private BTCommunicator btComm;
+
+	public SensorReportThread(String serverIp){
+		super("Sensor Report Thread");
+		this.serverIp = serverIp;
+		done = false;
+		connected = false;
+		btComm = BTCommunicator.getInstance();
+	}
+
+	@Override
+	public void run(){
+		byte[] lightReading;
+
+		if(connectToServer()){
+			while(!done){
+				if(btComm.isBTEnabled() && btComm.isConnected()){
+					try{
+						lightReading = btComm.readMessage(1);
+						writer.write(lightReading);
+					}catch(IOException io){
+						Logger.log_e(TAG, CLASS_NAME + "run(): IOException: " + io.getMessage());
+						done = true;
+					}
+				}else{
+					Logger.log_e(TAG, CLASS_NAME +  ".run() :: The robot disconnected or was never available.");
+					break;
+				}
+			}
+		}else{
+			Logger.log_e(TAG, CLASS_NAME + ".run() :: Could not connect to the server.");
+		}
+	}
+
+	public void finish(){
+		done = true;
+	}
+
+	private boolean connectToServer(){
+		boolean connected;
+		try{
+			socket = new Socket(serverIp, ProjectConstants.SENSOR_REPORT_PORT);
+			writer = socket.getOutputStream();
+			connected = true;
+		}catch(IOException io){
+			Logger.log_e(TAG, CLASS_NAME + ".connectToServer() :: IOException caught: " + io.getMessage());
+			connected = false;
+		}
+		return connected;
+	}
+
+	public boolean isConnected(){
+		return connected;
+	}
+}

src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java

@@ -17,15 +17,15 @@ package ve.ucv.ciens.ccg.nxtcam.network;
 
 import java.io.ByteArrayOutputStream;
 import java.io.IOException;
-import java.io.ObjectInputStream;
 import java.io.ObjectOutputStream;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
 import java.net.InetAddress;
 import java.net.Socket;
+import java.net.UnknownHostException;
 
 import ve.ucv.ciens.ccg.networkdata.VideoFrameDataMessage;
-import ve.ucv.ciens.ccg.networkdata.VideoStreamingControlMessage;
 import ve.ucv.ciens.ccg.nxtcam.camera.CameraImageMonitor;
-import ve.ucv.ciens.ccg.nxtcam.network.protocols.VideoStreamingProtocol;
 import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
 import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants;
 import android.graphics.ImageFormat;
@@ -36,222 +36,117 @@ public class VideoStreamingThread extends Thread{
 	private final String TAG = "IM_THREAD";
 	private final String CLASS_NAME = VideoStreamingThread.class.getSimpleName();
 
-	private enum ProtocolState_t {WAIT_FOR_ACK, WAIT_FOR_READY, CAN_SEND, END_STREAM};
-
 	private boolean pause, done;
 	private Object threadPauseMonitor;
 	private CameraImageMonitor camMonitor;
 	private Socket socket;
-	private ObjectOutputStream writer;
-	private ObjectInputStream reader;
+	DatagramSocket udpSocket;
 	private String serverIp;
-	private ProtocolState_t protocolState;
 	private final ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
 
 	public VideoStreamingThread(String serverIp){
 		super("Video Streaming Thread");
 		this.serverIp = serverIp;
-		pause = false;
 		done = false;
+		pause = false;
 		threadPauseMonitor = new Object();
 		socket = null;
-		writer = null;
-		reader = null;
 		camMonitor = CameraImageMonitor.getInstance();
-		protocolState = ProtocolState_t.WAIT_FOR_READY;
 	}
 
-	/*public void run(){
-		byte[] image;
-		Object tmpMessage;
-		VideoStreamingControlMessage controlMessage;
-		VideoFrameDataMessage dataMessage;
-		final ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
-
-		connectToServer();
-
-		if(!socket.isConnected()){
-			Logger.log_e(TAG, CLASS_NAME + ".run() :: Not connected to a server. Finishing thread.");
-			return;
-		}else{
-			while(!done){
-				// checkPause();
-				switch(protocolState){
-				case WAIT_FOR_READY:
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Reading message from server. State is WAIT_FOR_READY.");
-					tmpMessage = readMessage();
-
-					if(!validateImageTransferProtocolMessage(tmpMessage)){
-						// If the message received is not valid then send an UNRECOGNIZED message to the server.
-						Logger.log_d(TAG, CLASS_NAME + ".run() :: Received an unrecognized protocol message. State WAIT_FOR_READY.");
-						Logger.log_d(TAG, CLASS_NAME + ".run() :: Sending UNRECOGNIZED message to server.");
-						sendUnrecognizedMessage();
-
-					}else{
-						// Else if the message passed the validity check then proceed to the next protocol state.
-						controlMessage = (VideoStreamingControlMessage)tmpMessage;
-						if(controlMessage.message == VideoStreamingProtocol.FLOW_CONTROL_CONTINUE){
-							Logger.log_d(TAG, CLASS_NAME + ".run() :: Received FLOW_CONTROL_CONTINUE from the server.");
-							Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from WAIT_FOR_READY to CAN_SEND.");
-							protocolState = ProtocolState_t.CAN_SEND;
-						}else if(controlMessage.message == VideoStreamingProtocol.STREAM_CONTROL_END){
-							Logger.log_d(TAG, CLASS_NAME + ".run() :: Received STREAM_CONTROL_END from the server.");
-							Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from WAIT_FOR_READY to END_STREAM.");
-							protocolState = ProtocolState_t.END_STREAM;
-						}
-					}
-					break;
-
-				case WAIT_FOR_ACK:
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Reading message from server. State is WAIT_FOR_ACK.");
-					tmpMessage = readMessage();
-
-					if(!validateImageTransferProtocolMessage(tmpMessage)){
-						// If the message received is not valid then send an UNRECOGNIZED message to the server.
-						Logger.log_d(TAG, CLASS_NAME + ".run() :: Received an unrecognized protocol message. State WAIT_FOR_ACK.");
-						Logger.log_d(TAG, CLASS_NAME + ".run() :: Sending UNRECOGNIZED message to server.");
-						sendUnrecognizedMessage();
-
-					}else{
-						// Else if the message passed the validity check then proceed to the next protocol state.
-						controlMessage = (VideoStreamingControlMessage)tmpMessage;
-						if(controlMessage.message == VideoStreamingProtocol.ACK_SEND_NEXT){
-							Logger.log_d(TAG, CLASS_NAME + ".run() :: Received ACK_SEND_NEXT from the server.");
-							Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from WAIT_FOR_ACK to CAN_SEND.");
-							protocolState = ProtocolState_t.CAN_SEND;
-						}else if(controlMessage.message == VideoStreamingProtocol.ACK_WAIT){
-							Logger.log_d(TAG, CLASS_NAME + ".run() :: Received ACK_WAIT from the server.");
-							Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from WAIT_FOR_ACK to WAIT_FOR_READY.");
-							protocolState = ProtocolState_t.WAIT_FOR_READY;
-						}else if(controlMessage.message == VideoStreamingProtocol.STREAM_CONTROL_END){
-							protocolState = ProtocolState_t.END_STREAM;
-						}
-					}
-					break;
-
-				case CAN_SEND:
-					// Get the image and it's parameters from the monitor.
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Getting image data.");
-					Rect imageSize = camMonitor.getImageParameters();
-					image = camMonitor.getImageData();
-
-					// Compress the image as Jpeg.
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Compressing image.");
-					YuvImage yuvImage = new YuvImage(image, ImageFormat.NV21, imageSize.width(), imageSize.height(), null);
-					yuvImage.compressToJpeg(imageSize, 90, outputStream);
-
-					// Prepare the message for sending.
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Building message.");
-					dataMessage = new VideoFrameDataMessage();
-					dataMessage.imageWidth = imageSize.width();
-					dataMessage.imageHeight = imageSize.height();
-					dataMessage.data = outputStream.toByteArray();
-
-					// Send the message.
-					try{
-						Logger.log_d(TAG, CLASS_NAME + ".run() :: Sending message.");
-						writer.writeObject(dataMessage);
-					}catch(IOException io){
-						Logger.log_e(TAG, CLASS_NAME + ".run() :: Error sending image to the server: " + io.getMessage());
-					}
-
-					// Clean up stuff.
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Cleaning.");
-					yuvImage = null;
-					image = null;
-					outputStream.reset();
-					dataMessage = null;
-					imageSize = null;
-
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Image data successfuly sent.");
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from CAN_SEND to WAIT_FOR_ACK.");
-					protocolState = ProtocolState_t.WAIT_FOR_ACK;
-					break;
-
-				case END_STREAM:
-					// Simply disconnect from the server.
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Ending video stream.");
-					disconnect();
-					done = true;
-					break;
-				}
-			}
-		}
-		Logger.log_d(TAG, CLASS_NAME + ".run() :: Thread finish reached.");
-	}*/
-
 	public void run(){
-		connectToServer();
 
-		if(!socket.isConnected()){
-			Logger.log_e(TAG, CLASS_NAME + ".run() :: Not connected to a server. Finishing thread.");
-			return;
+		try{
+			udpSocket = new DatagramSocket();
 
-		}else{
-			while(!done){
-				sendImage();
-				try{
-					sleep(50L);
-				}catch(InterruptedException ie){}
-			}
+		}catch(IOException io){
+			Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException received creating socket " + io.getMessage());
+			System.exit(1);
 		}
 
+
+		while(!done){
+
+			synchronized (threadPauseMonitor) {
+				while(pause){
+					try{ threadPauseMonitor.wait(); }catch(InterruptedException ie){ };
+				}
+			}
+
+			sendUdp();
+			try{
+				sleep(50L);
+			}catch(InterruptedException ie){}
+		}
+
+
 		Logger.log_d(TAG, CLASS_NAME + ".run() :: Thread finish reached.");
 	}
 
-	private void sendImage(){
+	private byte[] int2ByteArray(int integer){
+		int shift;
+		byte[] array = new byte[4];
+		for(int i = 0; i < 4; i++){
+			shift = i << 3;
+			array[3 - i] = (byte)((integer & (0xff << shift)) >>> shift);
+		}
+		return array;
+	}
+
+	private void sendUdp(){
+		int bufferSize;
 		byte[] image;
-		YuvImage yuvImage;
+		byte[] buffer;
+		byte[] size;
+		DatagramPacket packet;
 		VideoFrameDataMessage message;
 		Rect imageSize;
+		YuvImage yuvImage;
 
 		image = camMonitor.getImageData();
-		if(image == null){
-			Logger.log_e(TAG, CLASS_NAME + ".sendImage() :: image is null, skipping frame.");
-			return;
-		}
 		imageSize = camMonitor.getImageParameters();
 
-		// Compress the image as Jpeg.
-		Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Compressing image.");
 		yuvImage = new YuvImage(image, ImageFormat.NV21, imageSize.width(), imageSize.height(), null);
 		yuvImage.compressToJpeg(imageSize, 90, outputStream);
 
-		Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Building message.");
 		message = new VideoFrameDataMessage();
 		message.data = outputStream.toByteArray();
 		message.imageWidth = imageSize.width();
 		message.imageHeight = imageSize.height();
 
-		try{
-			Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Sending message.");
-			writer.writeObject(message);
-			writer.flush();
-			writer.reset();
-			Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Message sent successfully: ");
-		}catch(IOException io){
-			Logger.log_e(TAG, CLASS_NAME + ".sendImage() :: Error sending image to the server: " + io.getMessage());
+		outputStream.reset();
 
-		}finally{
-			Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Cleaning.");
-			outputStream.reset();
-			image = null;
-			yuvImage = null;
-			message = null;
-			imageSize = null;
-			System.gc();
+		ByteArrayOutputStream baos = new ByteArrayOutputStream();
+
+		try{
+			ObjectOutputStream oos = new ObjectOutputStream(baos);
+			oos.writeObject(message);
+			oos.flush();
+			oos.reset();
+		}catch(IOException io){
+			Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: IOException received while serializing." + io.getMessage());
+			return;
 		}
-	}
 
-	private void connectToServer(){
+		buffer = baos.toByteArray();
+		baos.reset();
+		bufferSize = buffer.length;
+		size = int2ByteArray(bufferSize);
+
 		try{
-			Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connecting to the server at " + serverIp);
-			socket = new Socket(InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_1);
-			writer = new ObjectOutputStream(socket.getOutputStream());
-			reader = new ObjectInputStream(socket.getInputStream());
-			Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connection successful.");
+			packet = new DatagramPacket(size, 4, InetAddress.getByName(serverIp), ProjectConstants.VIDEO_STREAMING_PORT);
+			udpSocket.send(packet);
+
+			packet = new DatagramPacket(buffer, buffer.length, InetAddress.getByName(serverIp), ProjectConstants.VIDEO_STREAMING_PORT);
+			udpSocket.send(packet);
+
+		}catch(UnknownHostException uo){
+			Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: UnknownHostException received " + uo.getMessage());
+			return;
 		}catch(IOException io){
-			Logger.log_e(TAG, CLASS_NAME + ".connectToServer() :: Connection failed with message: " + io.getMessage());
+			Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: IOException buffer size is " + Integer.toString(buffer.length));
+			Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: IOException received while sending " + io.getMessage());
+			return;
 		}
 	}
 
@@ -271,56 +166,10 @@ public class VideoStreamingThread extends Thread{
 		Logger.log_i(TAG, CLASS_NAME + ".finish() :: Finishing thread.");
 	}
 
-	private void checkPause(){
-		synchronized (threadPauseMonitor){
-			while(pause){
-				Logger.log_d(TAG, CLASS_NAME + ".checkPause() :: Pause requested.");
-				try{ threadPauseMonitor.wait(); }catch(InterruptedException ie){}
-			}
+	public void pauseThread(){
+		synchronized (threadPauseMonitor) {
+			pause = true;
 		}
-	}
-
-	private Object readMessage(){
-		Object tmpMessage;
-
-		// Read a message from the server stream.
-		try{
-			tmpMessage = reader.readObject();
-
-		}catch(IOException io){
-			Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException when reading in WAIT_FOR_READY state.");
-			tmpMessage = null;
-			return null;
-		}catch(ClassNotFoundException cn){
-			Logger.log_e(TAG, CLASS_NAME + ".run() :: ClassNotFoundException when reading in WAIT_FOR_READY state.");
-			tmpMessage = null;
-			return null;
-		}
-
-		return tmpMessage;
-	}
-
-	private boolean validateImageTransferProtocolMessage(Object message){
-		if(message != null && message instanceof VideoStreamingControlMessage)
-			return true;
-		else
-			return false;
-	}
-
-	private void sendUnrecognizedMessage(){
-		VideoStreamingControlMessage message = new VideoStreamingControlMessage();
-		message.message = VideoStreamingProtocol.UNRECOGNIZED;
-
-		try{
-			writer.writeObject(message);
-		}catch(IOException io){
-			Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException when writing UNRECOGNIZED in WAIT_FOR_READY state.");
-		}
-		Logger.log_d(TAG, CLASS_NAME + ".run() :: UNRECOGNIZED message sent.");
-	}
-
-	public synchronized void pauseThread(){
-		pause = true;
 		Logger.log_d(TAG, CLASS_NAME + ".pauseThread() :: Pausing thread.");
 	}
 

src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java

@@ -16,7 +16,13 @@
 package ve.ucv.ciens.ccg.nxtcam.network.protocols;
 
 import java.security.InvalidParameterException;
+import java.util.Arrays;
 
+import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
+
+/**
+ * <p>Utility class that provides methods to get Lego Communication Protocol PDUs as byte arrays.</p> 
+ **/
 public abstract class LegoCommunicationProtocol{
 	/**
 	 * Command types. Byte 0;
@@ -68,33 +74,33 @@ public abstract class LegoCommunicationProtocol{
 	/**
 	 * Sensor types for setInputMode().
 	 */
-	public static final byte NO_SENSOR = 0x00;
-	public static final byte SWITCH = 0x01;
-	public static final byte TEMPERATURE = 0x02;
-	public static final byte REFLECTION = 0x03;
-	public static final byte ANGLE = 0x04;
-	public static final byte LIGHT_ACTIVE = 0x05;
-	public static final byte LIGHT_INACTIVE = 0x06;
-	public static final byte SOUND_DB = 0x07;
-	public static final byte SOUND_DBA = 0x08;
-	public static final byte CUSTOM = 0x09;
-	public static final byte LOWSPEED = 0x0A;
-	public static final byte LOWSPEED_9V = 0x0B;
+	public static final byte NO_SENSOR          = 0x00;
+	public static final byte SWITCH             = 0x01;
+	public static final byte TEMPERATURE        = 0x02;
+	public static final byte REFLECTION         = 0x03;
+	public static final byte ANGLE              = 0x04;
+	public static final byte LIGHT_ACTIVE       = 0x05;
+	public static final byte LIGHT_INACTIVE     = 0x06;
+	public static final byte SOUND_DB           = 0x07;
+	public static final byte SOUND_DBA          = 0x08;
+	public static final byte CUSTOM             = 0x09;
+	public static final byte LOWSPEED           = 0x0A;
+	public static final byte LOWSPEED_9V        = 0x0B;
 	public static final byte NO_OF_SENSOR_TYPES = 0x0C;
 
 	/**
 	 * Sensor modes for setInputMode().
 	 */
-	public static final byte RAWMODE = 0x00;
-	public static final byte BOOLEANMODE = 0x20;
-	public static final byte TRANSITIONCNTMODE = 0x40;
-	public static final byte PERIODCOUNTERMODE = 0x60;
-	public static final byte PCTFULLSCALEMODE = (byte)0x80;
-	public static final byte CELSIUSMODE = (byte)0xA0;
-	public static final byte FARENHEITMODE = (byte)0xC0;
-	public static final byte ANGLESTEPMODE = (byte)0xE0;
-	public static final byte SLOPEMASK = (byte)0x1F;
-	public static final byte MODEMASK = (byte)0xE0;
+	public static final byte RAWMODE           =       0x00;
+	public static final byte BOOLEANMODE       =       0x20;
+	public static final byte TRANSITIONCNTMODE =       0x40;
+	public static final byte PERIODCOUNTERMODE =       0x60;
+	public static final byte PCTFULLSCALEMODE  = (byte)0x80;
+	public static final byte CELSIUSMODE       = (byte)0xA0;
+	public static final byte FARENHEITMODE     = (byte)0xC0;
+	public static final byte ANGLESTEPMODE     = (byte)0xE0;
+	public static final byte SLOPEMASK         = (byte)0x1F;
+	public static final byte MODEMASK          = (byte)0xE0;
 
 	/**
 	 * Firmware and protocol version request pdu. Page 11 of appendix 1.
@@ -148,7 +154,7 @@ public abstract class LegoCommunicationProtocol{
 		if(turn_ratio < -100 || turn_ratio > 100){
 			throw new InvalidParameterException("Turn ratio out of range.");
 		}
-		if(mode_byte != MOTORON && mode_byte != BRAKE && mode_byte != REGULATED){
+		if(mode_byte < 0x00 || mode_byte > 0x07){
 			throw new InvalidParameterException("Invalid mode byte.");
 		}
 		if(regulation_mode != REGULATION_MODE_IDLE && regulation_mode != REGULATION_MODE_MOTOR_SPEED && regulation_mode != REGULATION_MODE_MOTOR_SYNC){
@@ -158,7 +164,7 @@ public abstract class LegoCommunicationProtocol{
 			throw new InvalidParameterException("Invalid run state.");
 		}
 
-		message[0] = 0x0C;
+		message[0] = 0x0D;
 		message[1] = 0x00;
 		message[2] = DIRECT_COMMAND_NO_REPLY;
 		message[3] = SET_OUTPUT_STATE;
@@ -170,9 +176,26 @@ public abstract class LegoCommunicationProtocol{
 		message[9] = run_state;
 		message[10] = message[11] = message[12] = message[13] = message[14] = 0x00; 
 
+		Logger.log_d("LCP", LegoCommunicationProtocol.class.getSimpleName() + "setOutputState(...) :: " + Arrays.toString(message));
+
 		return message;
 	}
 
+	/**
+	 * <p>Simpler call to the set motor configuration pdu method.</p>
+	 * 
+	 * @param output_port The port in the brick the motor is connected to.
+	 * @param power Motor power. Must be between -100 and 100.
+	 * @return The assembled pdu.
+	 */
+	public static byte[] setOutputState(byte output_port, byte power){
+		if(power == (byte)0){
+			return setOutputState(output_port, power, (byte)0x00, REGULATION_MODE_IDLE, (byte)0x00, MOTOR_RUN_STATE_IDLE);
+		}else{
+			return setOutputState(output_port, power, (byte)(MOTORON + BRAKE), REGULATION_MODE_MOTOR_SPEED, (byte)0x00, MOTOR_RUN_STATE_RUNNING);
+		}
+	}
+
 	/**
 	 * Motor configuration request pdu. Page 8 of appendix 2.
 	 * 

src/ve/ucv/ciens/ccg/nxtcam/network/protocols/MotorMasks.java

@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2014 Miguel Angel Astor Romero
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package ve.ucv.ciens.ccg.nxtcam.network.protocols;
+
+/**
+ * <p>Bit masks used to code/decode the control instructions sent by NxtAR-cam to
+ * NxtAR-bot.</p>
+ * 
+ * <p>Expansions 2-3 are currently unused.</p>
+ */
+public abstract class MotorMasks {
+	public static final byte MOTOR_A     = (byte)0x01;
+	public static final byte MOTOR_B     = (byte)0x02;
+	public static final byte MOTOR_C     = (byte)0x04;
+	public static final byte DIRECTION   = (byte)0x08;
+	public static final byte RECENTER    = (byte)0x10;
+	public static final byte ROTATE_90   = (byte)0x20;
+	public static final byte EXPANSION_2 = (byte)0x40;
+	public static final byte EXPANSION_3 = (byte)0x80;
+}

src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java

@@ -0,0 +1,61 @@
+package ve.ucv.ciens.ccg.nxtcam.robotcontrol;
+
+import java.util.LinkedList;
+import java.util.Queue;
+
+import ve.ucv.ciens.ccg.networkdata.MotorEvent;
+
+/**
+ * <p>A simple monitor class that encapsulates a queue.</p>
+ * <p>As it name says it stores motor events to be forwarded to the NXT robot.</p>
+ * <p>This class implements the singleton design pattern.<p> 
+ * 
+ * @author Miguel Angel Astor Romero
+ */
+public class MotorEventQueue {
+	/**
+	 * The event queue implemented as a linked list.
+	 */
+	private Queue<MotorEvent> motorEvents;
+
+	private MotorEventQueue(){
+		motorEvents = new LinkedList<MotorEvent>();
+	}
+
+	private static class SingletonHolder{
+		public static final MotorEventQueue instance = new MotorEventQueue();
+	}
+
+	/**
+	 * Return the singleton instance of this class.
+	 * @return The singleton instance.
+	 */
+	public static MotorEventQueue getInstance(){
+		return SingletonHolder.instance;
+	}
+
+	/**
+	 * <p>Get the first event on the queue.</p>
+	 * <p> If there are no events to return this method blocks until some thread calls the addEvent() method.</p>
+	 * @return The event at the front of the queue.
+	 */
+	public synchronized MotorEvent getNextEvent(){
+		while(motorEvents.size() == 0){
+			try{ wait(); }catch(InterruptedException ie){ }
+		}
+		return motorEvents.poll();
+	}
+
+	/**
+	 * <p>Adds an event to the back of the queue.</p>
+	 * @param event The event to add.
+	 */
+	public synchronized void addEvent(MotorEvent event){
+		motorEvents.add(event);
+		notifyAll();
+	}
+	
+	public synchronized int getSize(){
+		return motorEvents.size();
+	}
+}

src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java

@@ -21,9 +21,12 @@ import android.app.Activity;
 
 public abstract class ProjectConstants {
 	// Network related constants.
-	public static final int SERVER_UDP_PORT = 8889;
-	public static final int SERVER_TCP_PORT_1 = 9989;
-	public static final int SERVER_TCP_PORT_2 = 9990;
+	public static final int SERVICE_DISCOVERY_PORT = 9988;
+	public static final int VIDEO_STREAMING_PORT = 9989;
+	public static final int MOTOR_CONTROL_PORT = 9990;
+	public static final int SENSOR_REPORT_PORT = 9991;
+	public static final int APP_CONTROL_PORT = 9992;
+
 	public static final UUID SERIAL_PORT_SERVICE_CLASS_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
 	public static final String OUI_LEGO = "00:16:53";
 	public static final String MULTICAST_ADDRESS = "230.0.0.1";
@@ -32,5 +35,5 @@ public abstract class ProjectConstants {
 	// Activity results.
 	public static final int RESULT_CAMERA_FAILURE = Activity.RESULT_FIRST_USER + 1;
 
-	public static final boolean DEBUG = false;
+	public static final boolean DEBUG = true;
 }