WallyHackenslacker/NxtAR-cam
1
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

Started programming the robot control threads.

Browse Source
This commit is contained in:
Wally Hackenslacker
2014-01-20 12:59:51 -04:30
parent ab800a2558
commit f865c39cb7
9 changed files with 291 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
src/ve/ucv/ciens/ccg/networkdata/MotorEvent.java Normal file
View File

@@ -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;
}
}

8
src/ve/ucv/ciens/ccg/networkdata/SensorDataMessage.java Normal file
View File

@@ -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;
}

206
src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java
View File

@@ -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<BluetoothDevice> 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;
}
}
}

39
src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java
View File

@@ -15,13 +15,46 @@
*/ */
package ve.ucv.ciens.ccg.nxtcam.network; 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(){ 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;
} }
} }

80
src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java Normal file
View File

@@ -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;
}
}
}

48
src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java Normal file
View File

@@ -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;
}
}

51
src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java
View File

@@ -17,7 +17,6 @@ package ve.ucv.ciens.ccg.nxtcam.network;
import java.io.ByteArrayOutputStream; import java.io.ByteArrayOutputStream;
import java.io.IOException; import java.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream; import java.io.ObjectOutputStream;
import java.net.DatagramPacket; import java.net.DatagramPacket;
import java.net.DatagramSocket; import java.net.DatagramSocket;
@@ -26,9 +25,7 @@ import java.net.Socket;
import java.net.UnknownHostException; import java.net.UnknownHostException;
import ve.ucv.ciens.ccg.networkdata.VideoFrameDataMessage; 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.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.Logger;
import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants; import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants;
import android.graphics.ImageFormat; import android.graphics.ImageFormat;
@@ -39,30 +36,30 @@ public class VideoStreamingThread extends Thread{
private final String TAG = "IM_THREAD"; private final String TAG = "IM_THREAD";
private final String CLASS_NAME = VideoStreamingThread.class.getSimpleName(); 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 Object threadPauseMonitor;
private CameraImageMonitor camMonitor; private CameraImageMonitor camMonitor;
private Socket socket; private Socket socket;
DatagramSocket udpSocket; DatagramSocket udpSocket;
private ObjectOutputStream writer; /*private ObjectOutputStream writer;
private ObjectInputStream reader; private ObjectInputStream reader;*/
private String serverIp; private String serverIp;
private ProtocolState_t protocolState; //private ProtocolState_t protocolState;
private final ByteArrayOutputStream outputStream = new ByteArrayOutputStream(); private final ByteArrayOutputStream outputStream = new ByteArrayOutputStream();
public VideoStreamingThread(String serverIp){ public VideoStreamingThread(String serverIp){
super("Video Streaming Thread"); super("Video Streaming Thread");
this.serverIp = serverIp; this.serverIp = serverIp;
pause = false; //pause = false;
done = false; done = false;
threadPauseMonitor = new Object(); threadPauseMonitor = new Object();
socket = null; socket = null;
writer = null; //writer = null;
reader = null; //reader = null;
camMonitor = CameraImageMonitor.getInstance(); camMonitor = CameraImageMonitor.getInstance();
protocolState = ProtocolState_t.WAIT_FOR_READY; //protocolState = ProtocolState_t.WAIT_FOR_READY;
} }
/*public void run(){ /*public void run(){
@@ -186,7 +183,7 @@ public class VideoStreamingThread extends Thread{
public void run(){ public void run(){
connectToServer(); connectToServer();
try{ try{
udpSocket = new DatagramSocket(); udpSocket = new DatagramSocket();
udpSocket.setSendBufferSize(Integer.MAX_VALUE); udpSocket.setSendBufferSize(Integer.MAX_VALUE);
@@ -221,7 +218,7 @@ public class VideoStreamingThread extends Thread{
} }
return array; return array;
} }
private void sendUdp(){ private void sendUdp(){
int bufferSize; int bufferSize;
byte[] image; byte[] image;
@@ -231,20 +228,20 @@ public class VideoStreamingThread extends Thread{
VideoFrameDataMessage message; VideoFrameDataMessage message;
Rect imageSize; Rect imageSize;
YuvImage yuvImage; YuvImage yuvImage;
image = camMonitor.getImageData(); image = camMonitor.getImageData();
imageSize = camMonitor.getImageParameters(); imageSize = camMonitor.getImageParameters();
yuvImage = new YuvImage(image, ImageFormat.NV21, imageSize.width(), imageSize.height(), null); yuvImage = new YuvImage(image, ImageFormat.NV21, imageSize.width(), imageSize.height(), null);
yuvImage.compressToJpeg(imageSize, 90, outputStream); yuvImage.compressToJpeg(imageSize, 90, outputStream);
message = new VideoFrameDataMessage(); message = new VideoFrameDataMessage();
message.data = outputStream.toByteArray(); message.data = outputStream.toByteArray();
message.imageWidth = imageSize.width(); message.imageWidth = imageSize.width();
message.imageHeight = imageSize.height(); message.imageHeight = imageSize.height();
outputStream.reset(); outputStream.reset();
ByteArrayOutputStream baos = new ByteArrayOutputStream(); ByteArrayOutputStream baos = new ByteArrayOutputStream();
try{ try{
@@ -278,8 +275,8 @@ public class VideoStreamingThread extends Thread{
return; return;
} }
} }
private void sendImage(){ /*private void sendImage(){
byte[] image; byte[] image;
YuvImage yuvImage; YuvImage yuvImage;
VideoFrameDataMessage message; VideoFrameDataMessage message;
@@ -321,14 +318,14 @@ public class VideoStreamingThread extends Thread{
imageSize = null; imageSize = null;
System.gc(); System.gc();
} }
} }*/
private void connectToServer(){ private void connectToServer(){
try{ try{
Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connecting to the server at " + serverIp); 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.SERVER_TCP_PORT_1);
writer = new ObjectOutputStream(socket.getOutputStream()); /*writer = new ObjectOutputStream(socket.getOutputStream());
reader = new ObjectInputStream(socket.getInputStream()); reader = new ObjectInputStream(socket.getInputStream());*/
Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connection successful."); Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connection successful.");
}catch(IOException io){ }catch(IOException io){
Logger.log_e(TAG, CLASS_NAME + ".connectToServer() :: Connection failed with message: " + io.getMessage()); 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."); Logger.log_i(TAG, CLASS_NAME + ".finish() :: Finishing thread.");
} }
private void checkPause(){ /*private void checkPause(){
synchronized (threadPauseMonitor){ synchronized (threadPauseMonitor){
while(pause){ while(pause){
Logger.log_d(TAG, CLASS_NAME + ".checkPause() :: Pause requested."); 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_e(TAG, CLASS_NAME + ".run() :: IOException when writing UNRECOGNIZED in WAIT_FOR_READY state.");
} }
Logger.log_d(TAG, CLASS_NAME + ".run() :: UNRECOGNIZED message sent."); Logger.log_d(TAG, CLASS_NAME + ".run() :: UNRECOGNIZED message sent.");
} }*/
public synchronized void pauseThread(){ public synchronized void pauseThread(){
pause = true; //pause = true;
Logger.log_d(TAG, CLASS_NAME + ".pauseThread() :: Pausing thread."); Logger.log_d(TAG, CLASS_NAME + ".pauseThread() :: Pausing thread.");
} }
public synchronized void resumeThread(){ public synchronized void resumeThread(){
Logger.log_d(TAG, CLASS_NAME + ".resumeThread() :: Resuming thread."); Logger.log_d(TAG, CLASS_NAME + ".resumeThread() :: Resuming thread.");
synchronized (threadPauseMonitor) { synchronized (threadPauseMonitor) {
pause = false; //pause = false;
threadPauseMonitor.notifyAll(); threadPauseMonitor.notifyAll();
} }
} }

57
src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java Normal file
View File

@@ -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;
/**
* <p>A simple monitor class that encapsulates a queue.</p>
* <p>As it name says it stores motor events to be forwarded to the NXT robot.</p>
* <p>This class implements the singleton design pattern.<p>
*
* @author Miguel Angel Astor Romero
*/
public class MotorEventQueue {
/**
* The event queue implemented as a linked list.
*/
private Queue<MotorEvent> motorEvents;
private MotorEventQueue(){
motorEvents = new LinkedList<MotorEvent>();
}
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;
}
/**
* <p>Get the first event on the queue.</p>
* <p> If there are no events to return this method blocks until some thread calls the addEvent() method.</p>
* @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();
}
/**
* <p>Adds an event to the back of the queue.</p>
* @param event The event to add.
*/
public synchronized void addEvent(MotorEvent event){
motorEvents.add(event);
notifyAll();
}
}

1
src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java
View File

@@ -24,6 +24,7 @@ public abstract class ProjectConstants {
public static final int SERVER_UDP_PORT = 8889; public static final int SERVER_UDP_PORT = 8889;
public static final int SERVER_TCP_PORT_1 = 9989; 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_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 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 OUI_LEGO = "00:16:53";
public static final String MULTICAST_ADDRESS = "230.0.0.1"; public static final String MULTICAST_ADDRESS = "230.0.0.1";