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

Added logic for robot control thread management.

Browse Source
This commit is contained in:
Wally Hackenslacker
2014-02-11 17:52:29 -04:30
parent 89c744181e
commit 7db8c3fcee
7 changed files with 96 additions and 292 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java
View File

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

3
src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java
View File

@@ -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){

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

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

15
src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java
View File

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

12
src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java
View File

@@ -15,25 +15,31 @@ 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(){
if(connectToServer()){
while(!done){
}
}else{
Logger.log_e(TAG, CLASS_NAME + ".run() :: Could not connect to the server.");
}
}
public void finish(){
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;
}
}

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

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

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

@@ -20,6 +20,9 @@ import java.util.Arrays;
import ve.ucv.ciens.ccg.nxtcam.utils.Logger;
/**
* <p>Utility class that provides methods to get Lego Communication Protocol PDUs as byte arrays.</p>
**/
public abstract class LegoCommunicationProtocol{
/**
* Command types. Byte 0;
@@ -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;
}
/**
* <p>Simpler call to the set motor configuration pdu method.</p>
*
* @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);