LCPThread sends BT messages.

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

@@ -16,9 +16,8 @@
 package ve.ucv.ciens.ccg.nxtcam;
 
 import ve.ucv.ciens.ccg.nxtcam.camera.CameraPreview;
-import ve.ucv.ciens.ccg.nxtcam.network.MotorControlThread;
-import ve.ucv.ciens.ccg.nxtcam.network.VideoStreamingThread;
 import ve.ucv.ciens.ccg.nxtcam.network.LCPThread;
+import ve.ucv.ciens.ccg.nxtcam.network.VideoStreamingThread;
 import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
 import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants;
 import android.app.Activity;
@@ -42,7 +41,7 @@ public class CamActivity extends Activity{
 	private CameraSetupTask camSetupTask;
 	private VideoStreamingThread imThread;
 	private LCPThread botThread;
-	private MotorControlThread motorThread;
+	//private MotorControlThread motorThread;
 	private String serverIp;
 
 	/*******************
@@ -59,8 +58,11 @@ public class CamActivity extends Activity{
 		imThread = new VideoStreamingThread(serverIp);
 		imThread.start();
 
-		motorThread = new MotorControlThread(serverIp);
+		botThread = new LCPThread(serverIp);
-		motorThread.start();
+		botThread.start();
+		
+		/*motorThread = new MotorControlThread(serverIp);
+		motorThread.start();*/
 	}
 
 	@Override
@@ -116,8 +118,8 @@ public class CamActivity extends Activity{
 		imThread.finish();
 		imThread = null;
 		
-		motorThread.finish();
+		botThread.finish();
-		motorThread = null;
+		botThread = null;
 	}
 
 	@Override

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

@@ -15,6 +15,12 @@
  */
 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.LegoCommunicationProtocol;
+import ve.ucv.ciens.ccg.nxtcam.robotcontrol.MotorEventQueue;
 import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
 
 public class LCPThread extends Thread{
@@ -27,29 +33,63 @@ public class LCPThread extends Thread{
 	private MotorControlThread motorControl;
 	private SensorReportThread sensorReport;
 
+	private MotorEventQueue queue;
+
 	public LCPThread(String serverIp){
 		super("Robot Control Main Thread");
 		btComm = BTCommunicator.getInstance();
 		done = false;
 		motorControl = new MotorControlThread(serverIp);
 		sensorReport = new SensorReportThread(serverIp);
+		queue = MotorEventQueue.getInstance();
 	}
 
 	public void run(){
-		if(!motorControl.connectToServer()){
+		long then, now, delta;
-			Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server.");
+		MotorEvent event;
-			return;
+
+		//sensorReport.start();
+		motorControl.start();
+
+		then = System.currentTimeMillis();
+
+		while(!motorControl.isConnected()){
+			now = System.currentTimeMillis();
+			delta = now - then;
+			if(delta > 5000L){
+				Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server.");
+				return;
+			}
 		}
-		if(!(reportSensors = sensorReport.connectToServer())){
+		/*if(!(reportSensors = sensorReport.connectToServer())){
 			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.");
+				//Logger.log_d(TAG, CLASS_NAME + ".run() :: Connected.");
-				if(reportSensors)
+
-					Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported.");
+				event = queue.getNextEvent();
+
+				try{
+					btComm.writeMessage(
+							LegoCommunicationProtocol.setOutputState(
+									event.getMotor() == motor_t.MOTOR_A ? LegoCommunicationProtocol.PORT_0 : LegoCommunicationProtocol.PORT_2,
+											event.getPower())
+							);
+					Logger.log_i(TAG, CLASS_NAME + ".run() :: Message sent to the robot.");
+
+				}catch(IOException io){
+					Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException sending message to the robot.");
+				}
+
+				/*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

@@ -131,4 +131,8 @@ public class MotorControlThread extends Thread {
 			return null;
 		}
 	}
+	
+	public boolean isConnected(){
+		return connected;
+	}
 }

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

@@ -16,6 +16,9 @@
 package ve.ucv.ciens.ccg.nxtcam.network.protocols;
 
 import java.security.InvalidParameterException;
+import java.util.Arrays;
+
+import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
 
 public abstract class LegoCommunicationProtocol{
 	/**
@@ -148,9 +151,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 != MOTORON && mode_byte != BRAKE && mode_byte != REGULATED){
 			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.");
 		}
@@ -170,9 +173,19 @@ 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;
 	}
 
+	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.
 	 *