From 8e1d50ca32d0cec32b4ed4394c3e4ea87715115f Mon Sep 17 00:00:00 2001 From: Miguel Astor Date: Mon, 13 Jan 2014 14:31:52 -0430 Subject: [PATCH 1/8] Testing different frame sizes for transmission. --- src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java b/src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java index 176aa17..d90f08a 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java @@ -108,19 +108,19 @@ public class CameraPreview extends SurfaceView implements SurfaceHolder.Callback camParams.getSupportedPreviewSizes(); List sizes = camParams.getSupportedPreviewSizes(); - /*for(Size size: sizes){ + for(Size size: sizes){ Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged() :: Supported preview size (" + size.width + ", " + size.height + ")"); } - Size optimal = getOptimalPreviewSize(sizes, w, h); + /*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); + camParams.setPreviewSize(352, 288); camera.setParameters(camParams); /*previewWidth = optimal.width; previewHeight = optimal.height;*/ - previewWidth = 720; - previewHeight = 480; + previewWidth = 352; + previewHeight = 288; android.hardware.Camera.CameraInfo info = new android.hardware.Camera.CameraInfo(); android.hardware.Camera.getCameraInfo(0, info); From ab800a2558c1a25357b4276f82bac0b43be9129e Mon Sep 17 00:00:00 2001 From: Miguel Astor Date: Wed, 15 Jan 2014 14:49:58 -0430 Subject: [PATCH 2/8] Changed the streaming to use UDP. --- src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java | 1 - .../nxtcam/network/VideoStreamingThread.java | 82 ++++++++++++++++++- .../ccg/nxtcam/utils/ProjectConstants.java | 2 +- 3 files changed, 82 insertions(+), 3 deletions(-) diff --git a/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java b/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java index 880912d..31a5a1a 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java +++ b/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; diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java index 4e34b9c..9afdd13 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java @@ -19,8 +19,11 @@ 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; @@ -42,6 +45,7 @@ public class VideoStreamingThread extends Thread{ private Object threadPauseMonitor; private CameraImageMonitor camMonitor; private Socket socket; + DatagramSocket udpSocket; private ObjectOutputStream writer; private ObjectInputStream reader; private String serverIp; @@ -182,6 +186,14 @@ public class VideoStreamingThread extends Thread{ 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."); @@ -189,7 +201,8 @@ public class VideoStreamingThread extends Thread{ }else{ while(!done){ - sendImage(); + //sendImage(); + sendUdp(); try{ sleep(50L); }catch(InterruptedException ie){} @@ -199,6 +212,73 @@ public class VideoStreamingThread extends Thread{ Logger.log_d(TAG, CLASS_NAME + ".run() :: Thread finish reached."); } + 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; + byte[] buffer; + byte[] size; + DatagramPacket packet; + VideoFrameDataMessage message; + Rect imageSize; + YuvImage yuvImage; + + image = camMonitor.getImageData(); + imageSize = camMonitor.getImageParameters(); + + yuvImage = new YuvImage(image, ImageFormat.NV21, imageSize.width(), imageSize.height(), null); + yuvImage.compressToJpeg(imageSize, 90, outputStream); + + message = new VideoFrameDataMessage(); + message.data = outputStream.toByteArray(); + message.imageWidth = imageSize.width(); + message.imageHeight = imageSize.height(); + + outputStream.reset(); + + 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; + } + + buffer = baos.toByteArray(); + baos.reset(); + bufferSize = buffer.length; + size = int2ByteArray(bufferSize); + + try{ + packet = new DatagramPacket(size, 4, InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_2); + udpSocket.send(packet); + + packet = new DatagramPacket(buffer, buffer.length, InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_2); + 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 + ".sendUdp() :: IOException buffer size is " + Integer.toString(buffer.length)); + Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: IOException received while sending " + io.getMessage()); + return; + } + } + private void sendImage(){ byte[] image; YuvImage yuvImage; diff --git a/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java b/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java index 649bbd6..8569302 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java @@ -32,5 +32,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; } From f865c39cb79ba257c5e3ecf516e15f620b8a945b Mon Sep 17 00:00:00 2001 From: Miguel Astor Date: Mon, 20 Jan 2014 12:59:51 -0430 Subject: [PATCH 3/8] Started programming the robot control threads. --- .../ucv/ciens/ccg/networkdata/MotorEvent.java | 37 ++++ .../ccg/networkdata/SensorDataMessage.java | 8 + .../ccg/nxtcam/network/BluetoothManager.java | 206 ------------------ .../ciens/ccg/nxtcam/network/LCPThread.java | 39 +++- .../nxtcam/network/MotorControlThread.java | 80 +++++++ .../nxtcam/network/SensorReportThread.java | 48 ++++ .../nxtcam/network/VideoStreamingThread.java | 51 ++--- .../nxtcam/robotcontrol/MotorEventQueue.java | 57 +++++ .../ccg/nxtcam/utils/ProjectConstants.java | 1 + 9 files changed, 291 insertions(+), 236 deletions(-) create mode 100644 src/ve/ucv/ciens/ccg/networkdata/MotorEvent.java create mode 100644 src/ve/ucv/ciens/ccg/networkdata/SensorDataMessage.java delete mode 100644 src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java create mode 100644 src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java create mode 100644 src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java create mode 100644 src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java diff --git a/src/ve/ucv/ciens/ccg/networkdata/MotorEvent.java b/src/ve/ucv/ciens/ccg/networkdata/MotorEvent.java new file mode 100644 index 0000000..5cdcfc9 --- /dev/null +++ b/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}; + + 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; + } +} diff --git a/src/ve/ucv/ciens/ccg/networkdata/SensorDataMessage.java b/src/ve/ucv/ciens/ccg/networkdata/SensorDataMessage.java new file mode 100644 index 0000000..8f8617c --- /dev/null +++ b/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; + +} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java b/src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java deleted file mode 100644 index b330f8e..0000000 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java +++ /dev/null @@ -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 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; - } - } -} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java index 335f553..b0ee573 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java @@ -15,13 +15,46 @@ */ package ve.ucv.ciens.ccg.nxtcam.network; -public class LCPThread extends Thread{ +import ve.ucv.ciens.ccg.nxtcam.utils.Logger; - public LCPThread(){ - +public class LCPThread extends Thread{ + private static final String TAG = "LCP_THREAD"; + private static final String CLASS_NAME = LCPThread.class.getSimpleName(); + + private boolean done; + private boolean reportSensors; + private BTCommunicator btComm; + private MotorControlThread motorControl; + private SensorReportThread sensorReport; + + public LCPThread(String serverIp){ + super("Robot Control Main Thread"); + btComm = BTCommunicator.getInstance(); + done = false; + motorControl = new MotorControlThread(serverIp); + sensorReport = new SensorReportThread(serverIp); } public void run(){ + if(!motorControl.connectToServer()){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server."); + return; + } + 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."); + if(reportSensors) + Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported."); + } + } + } + + public void finish(){ + done = true; } } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java new file mode 100644 index 0000000..a71a4e4 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java @@ -0,0 +1,80 @@ +package ve.ucv.ciens.ccg.nxtcam.network; + +import java.io.IOException; +import java.io.ObjectInputStream; +import java.net.Socket; + +import ve.ucv.ciens.ccg.networkdata.MotorEvent; +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 boolean connected; + + public MotorControlThread(String serverIp){ + super("Motor Control Thread"); + this.serverIp = serverIp; + done = false; + connected = false; + queue = MotorEventQueue.getInstance(); + } + + @Override + public void run(){ + if(!connected){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: The thread is not connected to a server. Finishing."); + return; + }else{ + while(!done){ + + } + } + } + + public void finish(){ + done = true; + } + + public boolean connectToServer(){ + try{ + socket = new Socket(serverIp, ProjectConstants.SERVER_TCP_PORT_3); + reader = new ObjectInputStream(socket.getInputStream()); + 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(); + }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){ + return (MotorEvent)message; + }else{ + return null; + } + } +} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java new file mode 100644 index 0000000..4374b23 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java @@ -0,0 +1,48 @@ +package ve.ucv.ciens.ccg.nxtcam.network; + +import java.io.IOException; +import java.io.ObjectOutputStream; +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 ObjectOutputStream writer; + + public SensorReportThread(String serverIp){ + super("Sensor Report Thread"); + this.serverIp = serverIp; + done = false; + } + + @Override + public void run(){ + while(!done){ + + } + } + + public void finish(){ + done = true; + } + + public boolean connectToServer(){ + boolean connected; + try{ + socket = new Socket(serverIp, ProjectConstants.SERVER_TCP_PORT_3); + 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; + } +} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java index 9afdd13..2bed5e9 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java @@ -17,7 +17,6 @@ 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; @@ -26,9 +25,7 @@ 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; @@ -39,30 +36,30 @@ 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 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 ObjectOutputStream writer; + private ObjectInputStream reader;*/ private String serverIp; - private ProtocolState_t protocolState; + //private ProtocolState_t protocolState; private final ByteArrayOutputStream outputStream = new ByteArrayOutputStream(); public VideoStreamingThread(String serverIp){ super("Video Streaming Thread"); this.serverIp = serverIp; - pause = false; + //pause = false; done = false; threadPauseMonitor = new Object(); socket = null; - writer = null; - reader = null; + //writer = null; + //reader = null; camMonitor = CameraImageMonitor.getInstance(); - protocolState = ProtocolState_t.WAIT_FOR_READY; + //protocolState = ProtocolState_t.WAIT_FOR_READY; } /*public void run(){ @@ -186,7 +183,7 @@ public class VideoStreamingThread extends Thread{ public void run(){ connectToServer(); - + try{ udpSocket = new DatagramSocket(); udpSocket.setSendBufferSize(Integer.MAX_VALUE); @@ -221,7 +218,7 @@ public class VideoStreamingThread extends Thread{ } return array; } - + private void sendUdp(){ int bufferSize; byte[] image; @@ -231,20 +228,20 @@ public class VideoStreamingThread extends Thread{ VideoFrameDataMessage message; Rect imageSize; YuvImage yuvImage; - + image = camMonitor.getImageData(); imageSize = camMonitor.getImageParameters(); yuvImage = new YuvImage(image, ImageFormat.NV21, imageSize.width(), imageSize.height(), null); yuvImage.compressToJpeg(imageSize, 90, outputStream); - + message = new VideoFrameDataMessage(); message.data = outputStream.toByteArray(); message.imageWidth = imageSize.width(); message.imageHeight = imageSize.height(); - + outputStream.reset(); - + ByteArrayOutputStream baos = new ByteArrayOutputStream(); try{ @@ -278,8 +275,8 @@ public class VideoStreamingThread extends Thread{ return; } } - - private void sendImage(){ + + /*private void sendImage(){ byte[] image; YuvImage yuvImage; VideoFrameDataMessage message; @@ -321,14 +318,14 @@ public class VideoStreamingThread extends Thread{ 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()); + /*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()); @@ -351,7 +348,7 @@ public class VideoStreamingThread extends Thread{ Logger.log_i(TAG, CLASS_NAME + ".finish() :: Finishing thread."); } - private void checkPause(){ + /*private void checkPause(){ synchronized (threadPauseMonitor){ while(pause){ Logger.log_d(TAG, CLASS_NAME + ".checkPause() :: Pause requested."); @@ -397,17 +394,17 @@ public class VideoStreamingThread extends Thread{ 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; + //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/robotcontrol/MotorEventQueue.java b/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java new file mode 100644 index 0000000..8bc0cde --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java @@ -0,0 +1,57 @@ +package ve.ucv.ciens.ccg.nxtcam.robotcontrol; + +import java.util.LinkedList; +import java.util.Queue; + +import ve.ucv.ciens.ccg.networkdata.MotorEvent; + +/** + *

A simple monitor class that encapsulates a queue.

+ *

As it name says it stores motor events to be forwarded to the NXT robot.

+ *

This class implements the singleton design pattern.

+ * + * @author Miguel Angel Astor Romero + */ +public class MotorEventQueue { + /** + * The event queue implemented as a linked list. + */ + private Queue motorEvents; + + private MotorEventQueue(){ + motorEvents = new LinkedList(); + } + + 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; + } + + /** + *

Get the first event on the queue.

+ *

If there are no events to return this method blocks until some thread calls the addEvent() method.

+ * @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(); + } + + /** + *

Adds an event to the back of the queue.

+ * @param event The event to add. + */ + public synchronized void addEvent(MotorEvent event){ + motorEvents.add(event); + notifyAll(); + } +} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java b/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java index 8569302..1193c0e 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java @@ -24,6 +24,7 @@ public abstract class ProjectConstants { 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 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"; From 1c7430edd6fdb62ee773322d3dab523f7b854098 Mon Sep 17 00:00:00 2001 From: Miguel Astor Date: Tue, 21 Jan 2014 12:19:23 -0430 Subject: [PATCH 4/8] Removed the TCP video streaming code. --- src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java | 8 ++++---- .../ccg/nxtcam/network/MotorControlThread.java | 5 ++++- .../ccg/nxtcam/network/VideoStreamingThread.java | 14 +++++++------- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java index b0ee573..36aebcf 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java @@ -26,7 +26,7 @@ public class LCPThread extends Thread{ private BTCommunicator btComm; private MotorControlThread motorControl; private SensorReportThread sensorReport; - + public LCPThread(String serverIp){ super("Robot Control Main Thread"); btComm = BTCommunicator.getInstance(); @@ -34,7 +34,7 @@ public class LCPThread extends Thread{ motorControl = new MotorControlThread(serverIp); sensorReport = new SensorReportThread(serverIp); } - + public void run(){ if(!motorControl.connectToServer()){ Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server."); @@ -44,7 +44,7 @@ public class LCPThread extends Thread{ 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."); @@ -53,7 +53,7 @@ public class LCPThread extends Thread{ } } } - + public void finish(){ done = true; } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java index a71a4e4..d712060 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java @@ -35,7 +35,10 @@ public class MotorControlThread extends Thread { return; }else{ while(!done){ - + Object msg = readMessage(); + MotorEvent event = verifyMessage(msg); + if(event != null) + queue.addEvent(event); } } } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java index 2bed5e9..719189e 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java @@ -182,21 +182,21 @@ public class VideoStreamingThread extends Thread{ }*/ public void run(){ - connectToServer(); + //connectToServer(); try{ udpSocket = new DatagramSocket(); - udpSocket.setSendBufferSize(Integer.MAX_VALUE); + //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()){ + /*if(!socket.isConnected()){ Logger.log_e(TAG, CLASS_NAME + ".run() :: Not connected to a server. Finishing thread."); return; - }else{ + }else{*/ while(!done){ //sendImage(); sendUdp(); @@ -204,7 +204,7 @@ public class VideoStreamingThread extends Thread{ sleep(50L); }catch(InterruptedException ie){} } - } + //} Logger.log_d(TAG, CLASS_NAME + ".run() :: Thread finish reached."); } @@ -260,10 +260,10 @@ public class VideoStreamingThread extends Thread{ size = int2ByteArray(bufferSize); try{ - packet = new DatagramPacket(size, 4, InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_2); + packet = new DatagramPacket(size, 4, InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_1); udpSocket.send(packet); - packet = new DatagramPacket(buffer, buffer.length, InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_2); + packet = new DatagramPacket(buffer, buffer.length, InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_1); udpSocket.send(packet); }catch(UnknownHostException uo){ From bc9b44a849fdec254b24ff7e741c0fc5674f6db4 Mon Sep 17 00:00:00 2001 From: unknown Date: Tue, 4 Feb 2014 17:16:20 -0430 Subject: [PATCH 5/8] Changed settings. --- .settings/org.eclipse.jdt.core.prefs | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 .settings/org.eclipse.jdt.core.prefs diff --git a/.settings/org.eclipse.jdt.core.prefs b/.settings/org.eclipse.jdt.core.prefs new file mode 100644 index 0000000..b080d2d --- /dev/null +++ b/.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 From 6d6608bb67fa4a5f21fbbfd6a18eee3f13080ad8 Mon Sep 17 00:00:00 2001 From: unknown Date: Mon, 10 Feb 2014 15:24:34 -0430 Subject: [PATCH 6/8] MotorControlThread receives and sends data. --- .../ciens/ccg/networkdata/MotorEventACK.java | 32 ++++++++++ src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java | 14 ++++- src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java | 2 +- .../nxtcam/network/MotorControlThread.java | 61 +++++++++++++++++-- .../nxtcam/network/SensorReportThread.java | 2 +- .../nxtcam/network/VideoStreamingThread.java | 6 +- .../nxtcam/robotcontrol/MotorEventQueue.java | 4 ++ .../ccg/nxtcam/utils/ProjectConstants.java | 10 +-- 8 files changed, 114 insertions(+), 17 deletions(-) create mode 100644 src/ve/ucv/ciens/ccg/networkdata/MotorEventACK.java 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..6d28e8a 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java @@ -16,6 +16,7 @@ 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.utils.Logger; @@ -41,6 +42,7 @@ public class CamActivity extends Activity{ private CameraSetupTask camSetupTask; private VideoStreamingThread imThread; private LCPThread botThread; + private MotorControlThread motorThread; private String serverIp; /******************* @@ -56,6 +58,9 @@ public class CamActivity extends Activity{ serverIp = intent.getStringExtra("address"); imThread = new VideoStreamingThread(serverIp); imThread.start(); + + motorThread = new MotorControlThread(serverIp); + motorThread.start(); } @Override @@ -104,14 +109,17 @@ public class CamActivity extends Activity{ releaseCamera(); } } - + @Override public void onDestroy(){ super.onDestroy(); - // TODO: Destroy the network threads. + imThread.finish(); imThread = null; + + motorThread.finish(); + motorThread = 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/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java index d712060..12dcafb 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java @@ -2,9 +2,12 @@ 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 +21,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 +34,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 +97,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 +112,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,6 +125,7 @@ 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; diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java index 4374b23..ec33ef8 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java @@ -36,7 +36,7 @@ public class SensorReportThread extends Thread{ public 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){ diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java index 719189e..4c8c90f 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java @@ -260,10 +260,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){ @@ -323,7 +323,7 @@ public class VideoStreamingThread extends Thread{ 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); + 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."); 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"; From 89c744181e54ae0683b4a072b2e3fed55704ce94 Mon Sep 17 00:00:00 2001 From: unknown Date: Mon, 10 Feb 2014 16:36:32 -0430 Subject: [PATCH 7/8] LCPThread sends BT messages. --- src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java | 16 +++--- .../ciens/ccg/nxtcam/network/LCPThread.java | 56 ++++++++++++++++--- .../nxtcam/network/MotorControlThread.java | 4 ++ .../protocols/LegoCommunicationProtocol.java | 17 +++++- 4 files changed, 76 insertions(+), 17 deletions(-) diff --git a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java index 6d28e8a..58404ea 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java +++ b/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); - motorThread.start(); + botThread = new LCPThread(serverIp); + botThread.start(); + + /*motorThread = new MotorControlThread(serverIp); + motorThread.start();*/ } @Override @@ -116,8 +118,8 @@ public class CamActivity extends Activity{ imThread.finish(); imThread = null; - motorThread.finish(); - motorThread = null; + botThread.finish(); + botThread = null; } @Override diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java index 36aebcf..887f146 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,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()){ - 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 > 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."); - if(reportSensors) - Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported."); + //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.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; } } } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java index 12dcafb..9ed7cec 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java +++ b/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; + } } 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..3f6b56e 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java +++ b/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. * From 7db8c3fcee151bf6fc5863b94634b25fc4e4a1d1 Mon Sep 17 00:00:00 2001 From: unknown Date: Tue, 11 Feb 2014 17:52:29 -0430 Subject: [PATCH 8/8] Added logic for robot control thread management. --- src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java | 4 - .../ccg/nxtcam/network/BTCommunicator.java | 3 - .../ciens/ccg/nxtcam/network/LCPThread.java | 24 +- .../nxtcam/network/MotorControlThread.java | 15 + .../nxtcam/network/SensorReportThread.java | 14 +- .../nxtcam/network/VideoStreamingThread.java | 268 ++---------------- .../protocols/LegoCommunicationProtocol.java | 60 ++-- 7 files changed, 96 insertions(+), 292 deletions(-) diff --git a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java index 58404ea..48551d3 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java +++ b/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 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 887f146..e96ebc9 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java +++ b/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){ - //Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported."); - }*/ + 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 9ed7cec..cb770b2 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java +++ b/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; diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java index ec33ef8..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,7 +39,7 @@ public class SensorReportThread extends Thread{ 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; + } } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java index 4c8c90f..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."); } @@ -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(){ - 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 3f6b56e..f940c17 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java +++ b/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; +/** + *

Utility class that provides methods to get Lego Communication Protocol PDUs as byte arrays.

+ **/ public abstract class LegoCommunicationProtocol{ /** * Command types. Byte 0; @@ -71,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. @@ -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; } + /** + *

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);