Added logic for robot control thread management.

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

@@ -41,7 +41,6 @@ public class CamActivity extends Activity{
 	private CameraSetupTask camSetupTask;
 	private VideoStreamingThread imThread;
 	private LCPThread botThread;
-	//private MotorControlThread motorThread;
 	private String serverIp;
 
 	/*******************
@@ -60,9 +59,6 @@ public class CamActivity extends Activity{
 
 		botThread = new LCPThread(serverIp);
 		botThread.start();
-		
-		/*motorThread = new MotorControlThread(serverIp);
-		motorThread.start();*/
 	}
 
 	@Override

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/LCPThread.java

@@ -48,7 +48,7 @@ public class LCPThread extends Thread{
 		long then, now, delta;
 		MotorEvent event;
 
-		//sensorReport.start();
+		sensorReport.start();
 		motorControl.start();
 
 		then = System.currentTimeMillis();
@@ -56,37 +56,41 @@ public class LCPThread extends Thread{
 		while(!motorControl.isConnected()){
 			now = System.currentTimeMillis();
 			delta = now - then;
-			if(delta > 5000L){
+			if(delta > 9000L){
 				Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server.");
 				return;
 			}
 		}
-		/*if(!(reportSensors = sensorReport.connectToServer())){
+
+		if((reportSensors = sensorReport.isConnected())){
+			Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported.");
+		}else{
 			Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread sensorReport could not connect to the server.");
 			Logger.log_e(TAG, CLASS_NAME + ".run() :: Sensor data will not be reported to server app.");
-		}*/
+		}
 
 		while(!done){
 			if(btComm.isBTEnabled() && btComm.isConnected()){
-				//Logger.log_d(TAG, CLASS_NAME + ".run() :: Connected.");
 
 				event = queue.getNextEvent();
 
 				try{
 					btComm.writeMessage(
 							LegoCommunicationProtocol.setOutputState(
-									event.getMotor() == motor_t.MOTOR_A ? LegoCommunicationProtocol.PORT_0 : LegoCommunicationProtocol.PORT_2,
+									event.getMotor() == motor_t.MOTOR_A ? LegoCommunicationProtocol.PORT_0 : (event.getMotor() == motor_t.MOTOR_B ? LegoCommunicationProtocol.PORT_1 : LegoCommunicationProtocol.PORT_2),
 											event.getPower())
 							);
 					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.");
+					Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException sending message to the robot: " + io.getMessage());
 				}
 
-				/*if(reportSensors){
+				if(reportSensors){
-					//Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported.");
+					
-				}*/
+				}
 			}else{
 				Logger.log_e(TAG, CLASS_NAME +  ".run() :: The robot disconnected or was never available.");
 				break;

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

@@ -1,3 +1,18 @@
+/*
+ * 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;

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

@@ -15,25 +15,31 @@ public class SensorReportThread extends Thread{
 	private String serverIp;
 	private boolean done;
 	private ObjectOutputStream writer;
+	private boolean connected;
 
 	public SensorReportThread(String serverIp){
 		super("Sensor Report Thread");
 		this.serverIp = serverIp;
 		done = false;
+		connected = false;
 	}
 
 	@Override
 	public void run(){
+		if(connectToServer()){
 			while(!done){
 
 			}
+		}else{
+			Logger.log_e(TAG, CLASS_NAME + ".run() :: Could not connect to the server.");
+		}
 	}
 
 	public void finish(){
 		done = true;
 	}
 
-	public boolean connectToServer(){
+	private boolean connectToServer(){
 		boolean connected;
 		try{
 			socket = new Socket(serverIp, ProjectConstants.SENSOR_REPORT_PORT);
@@ -45,4 +51,8 @@ public class SensorReportThread extends Thread{
 		}
 		return connected;
 	}
+
+	public boolean isConnected(){
+		return connected;
+	}
 }

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

@@ -36,175 +36,49 @@ 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 boolean /*pause,*/ done;
 	private Object threadPauseMonitor;
 	private CameraImageMonitor camMonitor;
 	private Socket socket;
 	DatagramSocket udpSocket;
-	/*private ObjectOutputStream writer;
-	private ObjectInputStream reader;*/
 	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();
 
 		try{
 			udpSocket = new DatagramSocket();
-			//udpSocket.setSendBufferSize(Integer.MAX_VALUE);
+
 		}catch(IOException io){
 			Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException received creating socket " + io.getMessage());
 			System.exit(1);
 		}
 
-		/*if(!socket.isConnected()){
-			Logger.log_e(TAG, CLASS_NAME + ".run() :: Not connected to a server. Finishing thread.");
-			return;
 
-		}else{*/
 		while(!done){
-				//sendImage();
+
+			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.");
 	}
@@ -276,62 +150,6 @@ public class VideoStreamingThread extends Thread{
 		}
 	}
 
-	/*private void sendImage(){
-		byte[] image;
-		YuvImage yuvImage;
-		VideoFrameDataMessage message;
-		Rect imageSize;
-
-		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());
-
-		}finally{
-			Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Cleaning.");
-			outputStream.reset();
-			image = null;
-			yuvImage = null;
-			message = null;
-			imageSize = null;
-			System.gc();
-		}
-	}*/
-
-	private void connectToServer(){
-		try{
-			Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connecting to the server at " + serverIp);
-			socket = new Socket(InetAddress.getByName(serverIp), ProjectConstants.VIDEO_STREAMING_PORT);
-			/*writer = new ObjectOutputStream(socket.getOutputStream());
-			reader = new ObjectInputStream(socket.getInputStream());*/
-			Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connection successful.");
-		}catch(IOException io){
-			Logger.log_e(TAG, CLASS_NAME + ".connectToServer() :: Connection failed with message: " + io.getMessage());
-		}
-	}
-
 	public void disconnect(){
 		if(socket != null && socket.isConnected()){
 			try{
@@ -348,63 +166,17 @@ public class VideoStreamingThread extends Thread{
 		Logger.log_i(TAG, CLASS_NAME + ".finish() :: Finishing thread.");
 	}
 
-	/*private void checkPause(){
+	public void pauseThread(){
-		synchronized (threadPauseMonitor){
+		synchronized (threadPauseMonitor) {
-			while(pause){
+			pause = true;
-				Logger.log_d(TAG, CLASS_NAME + ".checkPause() :: Pause requested.");
-				try{ threadPauseMonitor.wait(); }catch(InterruptedException ie){}
 		}
-		}
-	}
-
-	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.");
 	}
 
 	public synchronized void resumeThread(){
 		Logger.log_d(TAG, CLASS_NAME + ".resumeThread() :: Resuming thread.");
 		synchronized (threadPauseMonitor) {
-			//pause = false;
+			pause = false;
 			threadPauseMonitor.notifyAll();
 		}
 	}

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

@@ -20,6 +20,9 @@ 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;
@@ -151,9 +154,9 @@ 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){
 			throw new InvalidParameterException("Invalid regulation mode.");
 		}
@@ -161,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;
@@ -178,6 +181,13 @@ public abstract class LegoCommunicationProtocol{
 		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);