diff --git a/src/ve/ucv/ciens/ccg/networkdata/MotorEventACK.java b/src/ve/ucv/ciens/ccg/networkdata/MotorEventACK.java
new file mode 100644
index 0000000..a2911f9
--- /dev/null
+++ b/src/ve/ucv/ciens/ccg/networkdata/MotorEventACK.java
@@ -0,0 +1,32 @@
+/*
+ * 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.networkdata;
+
+import java.io.Serializable;
+
+public class MotorEventACK implements Serializable {
+ private static final long serialVersionUID = 9989L;
+
+ private boolean clientQueueIsFull;
+
+ public MotorEventACK(boolean isQueueFull){
+ this.clientQueueIsFull = isQueueFull;
+ }
+
+ public boolean isClientQueueFull(){
+ return this.clientQueueIsFull;
+ }
+}
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java
index e02b331..48551d3 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java
+++ b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java
@@ -16,8 +16,8 @@
package ve.ucv.ciens.ccg.nxtcam;
import ve.ucv.ciens.ccg.nxtcam.camera.CameraPreview;
-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;
@@ -56,6 +56,9 @@ public class CamActivity extends Activity{
serverIp = intent.getStringExtra("address");
imThread = new VideoStreamingThread(serverIp);
imThread.start();
+
+ botThread = new LCPThread(serverIp);
+ botThread.start();
}
@Override
@@ -104,14 +107,17 @@ public class CamActivity extends Activity{
releaseCamera();
}
}
-
+
@Override
public void onDestroy(){
super.onDestroy();
- // TODO: Destroy the network threads.
+ imThread.finish();
imThread = null;
+
+ botThread.finish();
+ botThread = null;
}
-
+
@Override
public void onBackPressed(){
Intent result = new Intent();
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java b/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java
index 31a5a1a..651062b 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java
+++ b/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java
@@ -323,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){
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java b/src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java
index 6f6eedb..bf7ba25 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java
+++ b/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){
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java
index 36aebcf..e96ebc9 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java
+++ b/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,67 @@ 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()){
- Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server.");
- return;
+ long then, now, delta;
+ MotorEvent event;
+
+ sensorReport.start();
+ 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;
+ }
}
- 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.");
- 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 : (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: " + io.getMessage());
+ }
+
+ if(reportSensors){
+
+ }
+ }else{
+ Logger.log_e(TAG, CLASS_NAME + ".run() :: The robot disconnected or was never available.");
+ break;
}
}
}
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java
index d712060..cb770b2 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java
+++ b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java
@@ -1,10 +1,28 @@
+/*
+ * 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.MotorEvent.motor_t;
+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;
@@ -18,6 +36,7 @@ public class MotorControlThread extends Thread {
private MotorEventQueue queue;
private boolean done;
private ObjectInputStream reader;
+ private ObjectOutputStream writer;
private boolean connected;
public MotorControlThread(String serverIp){
@@ -30,15 +49,59 @@ public class MotorControlThread extends Thread {
@Override
public void run(){
- if(!connected){
+ 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){
- Object msg = readMessage();
- MotorEvent event = verifyMessage(msg);
- if(event != null)
+ // 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() == motor_t.MOTOR_A ? "MOTOR_A" : "MOTOR_C"));
+ 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());
}
}
}
@@ -49,8 +112,9 @@ public class MotorControlThread extends Thread {
public boolean connectToServer(){
try{
- socket = new Socket(serverIp, ProjectConstants.SERVER_TCP_PORT_3);
+ 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());
@@ -63,6 +127,7 @@ public class MotorControlThread extends Thread {
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;
@@ -75,9 +140,14 @@ public class MotorControlThread extends Thread {
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;
+ }
}
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java
index 4374b23..5bff72e 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java
+++ b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java
@@ -15,17 +15,23 @@ 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(){
- while(!done){
+ if(connectToServer()){
+ while(!done){
+ }
+ }else{
+ Logger.log_e(TAG, CLASS_NAME + ".run() :: Could not connect to the server.");
}
}
@@ -33,10 +39,10 @@ public class SensorReportThread extends Thread{
done = true;
}
- public boolean connectToServer(){
+ private boolean connectToServer(){
boolean connected;
try{
- socket = new Socket(serverIp, ProjectConstants.SERVER_TCP_PORT_3);
+ socket = new Socket(serverIp, ProjectConstants.SENSOR_REPORT_PORT);
writer = new ObjectOutputStream(socket.getOutputStream());
connected = true;
}catch(IOException io){
@@ -45,4 +51,8 @@ public class SensorReportThread extends Thread{
}
return connected;
}
+
+ public boolean isConnected(){
+ return connected;
+ }
}
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java
index 719189e..91bdf27 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java
+++ b/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();
- sendUdp();
- try{
- sleep(50L);
- }catch(InterruptedException ie){}
+ 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.");
}
@@ -260,10 +134,10 @@ public class VideoStreamingThread extends Thread{
size = int2ByteArray(bufferSize);
try{
- packet = new DatagramPacket(size, 4, InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_1);
+ 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.SERVER_TCP_PORT_1);
+ packet = new DatagramPacket(buffer, buffer.length, InetAddress.getByName(serverIp), ProjectConstants.VIDEO_STREAMING_PORT);
udpSocket.send(packet);
}catch(UnknownHostException uo){
@@ -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.SERVER_TCP_PORT_1);
- /*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(){
- 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.");
}
public synchronized void resumeThread(){
Logger.log_d(TAG, CLASS_NAME + ".resumeThread() :: Resuming thread.");
synchronized (threadPauseMonitor) {
- //pause = false;
+ pause = false;
threadPauseMonitor.notifyAll();
}
}
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java b/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java
index 404c63a..f940c17 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java
+++ b/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;
+
+/**
+ * Utility class that provides methods to get Lego Communication Protocol PDUs as byte arrays.
+ **/
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;
}
+ /**
+ * Simpler call to the set motor configuration pdu method.
+ *
+ * @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.
*
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java b/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java
index 8bc0cde..2e77cce 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java
+++ b/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java
@@ -54,4 +54,8 @@ public class MotorEventQueue {
motorEvents.add(event);
notifyAll();
}
+
+ public synchronized int getSize(){
+ return motorEvents.size();
+ }
}
diff --git a/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java b/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java
index 1193c0e..d86dd31 100644
--- a/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java
+++ b/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java
@@ -21,10 +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 SERVER_TCP_PORT_3 = 9991;
+ 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";