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