From f865c39cb79ba257c5e3ecf516e15f620b8a945b Mon Sep 17 00:00:00 2001 From: Miguel Astor Date: Mon, 20 Jan 2014 12:59:51 -0430 Subject: [PATCH] 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";