Merge branch 'develop'

src/ve/ucv/ciens/icaro/libnxtarcontrol/NxtARControlProtocol.java

@@ -14,6 +14,8 @@ package ve.ucv.ciens.icaro.libnxtarcontrol;
 
 import java.io.DataInput;
 import java.io.DataInputStream;
+import java.io.DataOutput;
+import java.io.DataOutputStream;
 import java.io.IOException;
 import java.util.LinkedList;
 
@@ -28,7 +30,7 @@ import ve.ucv.ciens.icaro.libnxtarcontrol.DecodedControlAction.Motor;
  * @see <a href="http://www.lejos.org">The LejOS operating system.</a>
  * @see <a href="https://github.com/sagge-miky/NxtAR-core">NxtAR-core Github repository.</a>
  * @author Miguel Angel Astor Romero
- * @version 1.0.1
+ * @version 1.2.0
  * @since December 15, 2014
  */
 public class NxtARControlProtocol {
@@ -53,64 +55,198 @@ public class NxtARControlProtocol {
 	private static final byte USER_2      = (byte)0x40;
 	private static final byte USER_3      = (byte)0x80;
 
+	// Helpfull masks.
+	private static final byte STOP_ALL_MOTORS = (byte)0xF8;
+	private static final byte MOVE_BACKWARD = (byte)0xF8;
+
 	private DataInputStream inputStream;
+	private DataOutputStream outputStream;
 	private LinkedList<UserActionListener> userActionListeners;
 
 	/**
 	 * <p>Create a new ARControl object.</p> 
 	 * 
-	 * @param inputStream An opened input stream used to read protocol messages from.
-	 * @throws IllegalArgumentException If inputStream is null.
+	 * @param inputStream A {@link DataInputStream} used to read protocol messages from. Can be null.
+	 * @param outputStream An {@link DataOutputStream} used to write protocol messages to. Can be null.
 	 */
-	public NxtARControlProtocol(final DataInputStream inputStream) throws IllegalArgumentException{
-		if(inputStream == null)
-			throw new IllegalArgumentException("Input stream is null.");
-
+	public NxtARControlProtocol(final DataInputStream inputStream, final DataOutputStream outputStream){
+		this.outputStream = outputStream;
 		this.inputStream = inputStream;
 		this.userActionListeners = new LinkedList<UserActionListener>();
 	}
 
 	/**
-	 * <p>Changes the input stream associated with this ARControl for the input stream passed as
-	 * parameter. The currently set input stream is closed before replacing it.</p>
+	 * <p>Changes the {@link DataOutputStream} associated with this ARControl to the output stream passed as
+	 * parameter. The currently set output stream is flushed and closed before replacing it.</p>
 	 * 
-	 * @param inputStream An opened input stream.
-	 * @throws IOException If an error happened while closing the previous input stream.
-	 * @throws IllegalArgumentException If the input stream is null.
+	 * @param outpuStream An opened output stream. Can be null.
+	 * @throws IOException If an error happened while closing the previous output stream.
 	 */
-	public void setInputStream(DataInputStream inputStream) throws IOException, IllegalArgumentException{
-		if(inputStream == null)
-			throw new IllegalArgumentException("Input stream is null.");
-
-		try{
-			this.inputStream.close();
-		}catch(IOException io){
-			throw io;
+	public void setOutputStream(DataOutputStream outputStream) throws IOException{
+		if(this.outputStream != null){
+			this.outputStream.flush();
+			this.outputStream.close();
+			this.outputStream = null;
 		}
 
+		this.outputStream = outputStream;
+	}
+
+	/**
+	 * <p>Attempts to write a 2-byte message to the associated {@link DataOutputStream} if any.</p>
+	 * 
+	 * <p>Returns immediately if no {@link DataOutputStream} has been set with
+	 * {@link NxtARControlProtocol#setOutputStream(DataOutputStream)} or if the message is null. If the
+	 * message is longer than two bytes only the first two bytes are written to the output stream.</p>
+	 * 
+	 * @throws IOException If writing the message fails. It is the same IOException
+	 * as thrown by {@link DataOutput#write(byte[], int, int)}.
+	 * @throws IllegalArgumentException If the message lenght is less than two.
+	 */
+	public void writeRawControlMessage(byte[] message) throws IOException, IllegalArgumentException{
+		if(outputStream == null || message == null){
+			return;
+		}else{
+			if(message.length < 2)
+				throw new IllegalArgumentException("Message length is less than two.");
+
+			synchronized (outputStream) {
+				outputStream.write(message, 0, 2);
+				outputStream.flush();
+			}
+		}
+	}
+
+	/**
+	 * <p>Encodes a given {@link DecodedControlAction} into a two byte array.</p>
+	 * 
+	 * @param action the action to encode.
+	 * @return The encoded action. Null if said action is null.
+	 */
+	public byte[] encodeControlAction(DecodedControlAction action){
+		if(action == null){
+			return null;
+		}else{
+			byte[] message = {0x00, 0x00};
+
+			switch(action.action){
+			case MOVE_BACKWARDS:
+				message[0] |= DIRECTION;
+				break;
+			case MOVE_FORWARD:
+				message[0] &= MOVE_BACKWARD;
+				break;
+			case RECENTER:
+				message[0] |= RECENTER;
+				break;
+			case STOP:
+				message[0] &= STOP_ALL_MOTORS; 
+				break;
+			case USER_1:
+				message[0] |= USER_1;
+				break;
+			case USER_2:
+				message[0] |= USER_2;
+				break;
+			case USER_3:
+				message[0] |= USER_3;
+				break;
+			}
+
+			switch(action.motor){
+			case MOTOR_A:
+				message[0] |= MOTOR_A;
+				break;
+			case MOTOR_AB:
+				message[0] |= MOTOR_A;
+				message[0] |= MOTOR_B;
+				break;
+			case MOTOR_ABC:
+				message[0] |= MOTOR_A;
+				message[0] |= MOTOR_B;
+				message[0] |= MOTOR_C;
+				break;
+			case MOTOR_AC:
+				message[0] |= MOTOR_A;
+				message[0] |= MOTOR_C;
+				break;
+			case MOTOR_B:
+				message[0] |= MOTOR_B;
+				break;
+			case MOTOR_BC:
+				message[0] |= MOTOR_B;
+				message[0] |= MOTOR_C;
+				break;
+			case MOTOR_C:
+				message[0] |= MOTOR_C;
+				break;
+			}
+
+			message[1] = (byte)clamp(action.speed, -100, 100);
+
+			return message;
+		}
+	}
+
+	/**
+	 * <p>Encodes and writes a {@link DecodedControlAction} into the associated {@link DataOutputStream} if any.</p>
+	 * 
+	 * @param action The action to write.
+	 * @return True if writing the action succeded. False if either the action or the DataOutputStream are null.
+	 * @throws IOException If writing the message fails.
+	 */
+	public boolean writeMessage(final DecodedControlAction action) throws IOException{
+		boolean success = false;
+
+		if(action != null && outputStream != null){
+			byte[] msg = encodeControlAction(action);
+
+			writeRawControlMessage(msg);
+			success = true;
+		}
+
+		return success;
+	}
+
+	/**
+	 * <p>Changes the input stream associated with this ARControl to the input stream passed as
+	 * parameter. The currently set input stream is closed before replacing it.</p>
+	 * 
+	 * @param inputStream An opened input stream. Can be null.
+	 * @throws IOException If an error happened while closing the previous input stream.
+	 */
+	public void setInputStream(DataInputStream inputStream) throws IOException{
+		if(this.inputStream != null){
+			this.inputStream.close();
 			this.inputStream = null;
+		}
+
 		this.inputStream = inputStream;
 	}
 
 	/**
 	 * <p>Attempts to read a 2-byte message and returns it as is.</p>
 	 * 
+	 * <p>Returns null if no {@link DataInputStream} has been set with
+	 * {@link NxtARControlProtocol#setInputStream(DataInputStream)}.</p>
+	 * 
 	 * @return The two bytes read from the associated connection as an array.
 	 * @throws IOException If reading the message fails. It is the same IOException
-	 * as thrown by {@link DataInput#readByte()} if any.
+	 * as thrown by {@link DataInput#readByte()}.
 	 */
 	public byte[] readRawControlMessage() throws IOException{
+		if(inputStream == null){
+			return null;
+		}else{
 			byte[] msg = new byte[2];
-		try{
 			synchronized (inputStream) {
 				msg[0] = inputStream.readByte();
 				msg[1] = inputStream.readByte();
 			}
-		}catch (IOException io){
-			throw io;
-		}
+
 			return msg;
 		}
+	}
 
 	/**
 	 * <p>Attempts to read, decode and execute a message, calling the user operation
@@ -140,6 +276,111 @@ public class NxtARControlProtocol {
 			}
 
 			if(controlAction != null){
+				success = executeControlAction(controlAction);
+			}
+		}
+
+		return success;
+	}
+
+	/**
+	 * <p>Decodes a protocol message encoded as a byte array of two elements as specified
+	 * in the package definition.</p>
+	 * 
+	 * <p>User actions have precedence over motor recentering and
+	 * this in turn has precedence over other movement actions.
+	 * User actions have precedence in decreasing order; that is, user action 1 has
+	 * precedence over user actions 2 and 3, etc.</p>
+	 * 
+	 * <p>If the message indicates a movement (forward or backward) with all motors off,
+	 * then it is interpreted as a request to stop all motors. A recenter or user action
+	 * with all motors off will be decoded as is and must be interpreted by the user.</p>
+	 * 
+	 * @param message A byte array of size two encoding a message recognized by the protocol. If the array
+	 * has 3 or more elements then only the first 2 are used during the decoding process. 
+	 * @return A {@link DecodedControlAction} instance containing the decoded message.
+	 * @throws IllegalArgumentException If the array is null or has less than 2 elements.
+	 */
+	public DecodedControlAction decodeMessage(final byte[] message) throws IllegalArgumentException{
+		Action action = Action.STOP;
+		Motor motor = Motor.MOTOR_ABC;
+		DecodedControlAction controlAction;
+
+		if(message == null)
+			throw new IllegalArgumentException("Message is null.");
+
+		if(message.length < 2)
+			throw new IllegalArgumentException("Message is too short. Length of message is " + message.length);
+
+		// Decode the message.
+		boolean motorA    = (message[0] & MOTOR_A)   > 0 ? true : false;
+		boolean motorB    = (message[0] & MOTOR_B)   > 0 ? true : false;
+		boolean motorC    = (message[0] & MOTOR_C)   > 0 ? true : false;
+		boolean recenter  = (message[0] & RECENTER)  > 0 ? true : false;
+		boolean user1     = (message[0] & USER_1)    > 0 ? true : false;
+		boolean user2     = (message[0] & USER_2)    > 0 ? true : false;
+		boolean user3     = (message[0] & USER_3)    > 0 ? true : false;
+		int     direction = (message[0] & DIRECTION) > 0 ? BasicMotorPort.FORWARD : BasicMotorPort.BACKWARD;
+
+		// Set the action flag. 
+		if(user1 || user2 || user3){
+			if     (user1) action = Action.USER_1;
+			else if(user2) action = Action.USER_2;
+			else if(user3) action = Action.USER_3;
+		}else if(recenter){
+			action = Action.RECENTER;
+		}else{
+			if(direction == BasicMotorPort.FORWARD){
+				action = Action.MOVE_FORWARD;
+			}else if(direction == BasicMotorPort.BACKWARD){
+				action = Action.MOVE_BACKWARDS;
+			}
+		}
+
+		// Set the motor flag.
+		if(motorA && motorB && motorC){
+			motor = Motor.MOTOR_ABC;
+		}else if(motorA && motorB && !motorC){
+			motor = Motor.MOTOR_AB;
+		}else if(motorA && !motorB && motorC){
+			motor = Motor.MOTOR_AC;
+		}else if(!motorA && motorB && motorC){
+			motor = Motor.MOTOR_BC;
+		}else if(motorA && !motorB && !motorC){
+			motor = Motor.MOTOR_A;
+		}else if(!motorA && motorB && !motorC){
+			motor = Motor.MOTOR_B;
+		}else if(!motorA && !motorB && motorC){
+			motor = Motor.MOTOR_C;
+		}
+
+		// Check for stop condition.
+		if(action == Action.MOVE_FORWARD || action == Action.MOVE_BACKWARDS){
+			if(!motorA && !motorB && !motorC){
+				action = Action.STOP;
+				motor = Motor.MOTOR_ABC;
+			}
+		}
+
+		controlAction = new DecodedControlAction(action, motor, message[1]);
+
+		return controlAction;
+	}
+
+	/**
+	 * <p>Executes an already decoded {@link DecodedControlAction}, calling the user operation
+	 * listeners if needed.</p>
+	 * 
+	 * @param controlAction The action to execute.
+	 * @return True if the action could be executed successfully. False otherwise.
+	 * @throws IllegalArgumentException If controlAction is null.
+	 */
+	public boolean executeControlAction(final DecodedControlAction controlAction) throws IllegalArgumentException{
+		boolean success = false;
+
+		if(controlAction == null){
+			throw new IllegalArgumentException("Control action is null.");
+		}else{
 			switch(controlAction.action){
 			case MOVE_BACKWARDS:
 				switch(controlAction.motor){
@@ -277,95 +518,10 @@ public class NxtARControlProtocol {
 				break;
 			}
 		}
-		}
 
 		return success;
 	}
 
-	/**
-	 * <p>Decodes a protocol message encoded as a byte array of two elements as specified
-	 * in the package definition.</p>
-	 * 
-	 * <p>User actions have precedence over motor recentering and
-	 * this in turn has precedence over other movement actions.
-	 * User actions have precedence in decreasing order; that is, user action 1 has
-	 * precedence over user actions 2 and 3, etc.</p>
-	 * 
-	 * <p>If the message indicates a movement (forward or backward) with all motors off,
-	 * then it is interpreted as a request to stop all motors. A recenter or user action
-	 * with all motors off will be decoded as is and must be interpreted by the user.</p>
-	 * 
-	 * @param message A byte array of size two encoding a message recognized by the protocol. If the array
-	 * has 3 or more elements then only the first 2 are used during the decoding process. 
-	 * @return A {@link DecodedControlAction} instance containing the decoded message.
-	 * @throws IllegalArgumentException If the array is null or has less than 2 elements.
-	 */
-	public DecodedControlAction decodeMessage(byte[] message) throws IllegalArgumentException{
-		Action action = Action.STOP;
-		Motor motor = Motor.MOTOR_ABC;
-		DecodedControlAction controlAction;
-
-		if(message == null)
-			throw new IllegalArgumentException("Message is null.");
-
-		if(message.length < 2)
-			throw new IllegalArgumentException("Message is too short. Length of message is " + message.length);
-
-		// Decode the message.
-		boolean motorA    = (message[0] & MOTOR_A)   > 0 ? true : false;
-		boolean motorB    = (message[0] & MOTOR_B)   > 0 ? true : false;
-		boolean motorC    = (message[0] & MOTOR_C)   > 0 ? true : false;
-		boolean recenter  = (message[0] & RECENTER)  > 0 ? true : false;
-		boolean user1     = (message[0] & USER_1)    > 0 ? true : false;
-		boolean user2     = (message[0] & USER_2)    > 0 ? true : false;
-		boolean user3     = (message[0] & USER_3)    > 0 ? true : false;
-		int     direction = (message[0] & DIRECTION) > 0 ? BasicMotorPort.FORWARD : BasicMotorPort.BACKWARD;
-
-		// Set the action flag. 
-		if(user1 || user2 || user3){
-			if     (user1) action = Action.USER_1;
-			else if(user2) action = Action.USER_2;
-			else if(user3) action = Action.USER_3;
-		}else if(recenter){
-			action = Action.RECENTER;
-		}else{
-			if(direction == BasicMotorPort.FORWARD){
-				action = Action.MOVE_FORWARD;
-			}else if(direction == BasicMotorPort.BACKWARD){
-				action = Action.MOVE_BACKWARDS;
-			}
-		}
-
-		// Set the motor flag.
-		if(motorA && motorB && motorC){
-			motor = Motor.MOTOR_ABC;
-		}else if(motorA && motorB && !motorC){
-			motor = Motor.MOTOR_AB;
-		}else if(motorA && !motorB && motorC){
-			motor = Motor.MOTOR_AC;
-		}else if(!motorA && motorB && motorC){
-			motor = Motor.MOTOR_BC;
-		}else if(motorA && !motorB && !motorC){
-			motor = Motor.MOTOR_A;
-		}else if(!motorA && motorB && !motorC){
-			motor = Motor.MOTOR_B;
-		}else if(!motorA && !motorB && motorC){
-			motor = Motor.MOTOR_C;
-		}
-
-		// Check for stop condition.
-		if(action == Action.MOVE_FORWARD || action == Action.MOVE_BACKWARDS){
-			if(!motorA && !motorB && !motorC){
-				action = Action.STOP;
-				motor = Motor.MOTOR_ABC;
-			}
-		}
-
-		controlAction = new DecodedControlAction(action, motor, message[1]);
-
-		return controlAction;
-	}
-
 	/**
 	 * <p>Adds an {@link UserActionListener} to this object's listeners list calling it's 
 	 * {@link UserActionListener#onListenerRegistered()} method. Adding a listener that
@@ -374,7 +530,7 @@ public class NxtARControlProtocol {
 	 * @param listener The listener to add.
 	 * @throws IllegalArgumentException If listener is null.
 	 */
-	public synchronized void registerUserActionListener(UserActionListener listener) throws IllegalArgumentException{
+	public synchronized void registerUserActionListener(final UserActionListener listener) throws IllegalArgumentException{
 		if(listener == null)
 			throw new IllegalArgumentException("Listener is null.");
 
@@ -392,7 +548,7 @@ public class NxtARControlProtocol {
 	 * @param listener The listener to remove.
 	 * @throws IllegalArgumentException If listener is null.
 	 */
-	public synchronized void removeUserActionListener(UserActionListener listener) throws IllegalArgumentException{
+	public synchronized void removeUserActionListener(final UserActionListener listener) throws IllegalArgumentException{
 		if(listener == null)
 			throw new IllegalArgumentException("Listener is null.");
 
@@ -409,7 +565,7 @@ public class NxtARControlProtocol {
 	 * 
 	 * @param userAction The action that triggered the notification.
 	 */
-	private void notifyListeners(Action userAction, Motor motorFlag, int speed){
+	private void notifyListeners(final Action userAction, final Motor motorFlag, final int speed){
 		switch(userAction){
 		case USER_1:
 			for(UserActionListener listener : userActionListeners)
@@ -519,4 +675,8 @@ public class NxtARControlProtocol {
 			break;
 		}
 	}
+
+	private int clamp(int i, int min, int max){
+		return i > max ? max : (i < min ? min : i);
+	}
 }

src/ve/ucv/ciens/icaro/libnxtarcontrol/UserActionListener.java

@@ -24,7 +24,7 @@ import ve.ucv.ciens.icaro.libnxtarcontrol.DecodedControlAction.Motor;
  */
 public interface UserActionListener {
 	/**
-	 * <p>Executes a set of instructions just after the listener has been registered with an {@link NxtARControlProtocol instance}.</p>
+	 * <p>Executes a set of instructions just after the listener has been registered with an {@link NxtARControlProtocol} instance.</p>
 	 */
 	public void onListenerRegistered();
 
@@ -44,7 +44,7 @@ public interface UserActionListener {
 	public void onUserAction3(Motor motorFlag, int speed);
 
 	/**
-	 * <p>Executes a set of instructions just after the listener has been removed from an {@link NxtARControlProtocol instance}.</p>
+	 * <p>Executes a set of instructions just after the listener has been removed from an {@link NxtARControlProtocol} instance.</p>
 	 */
 	public void onListenerRemoved();
 }