6 Commits
1.0 ... 1.2.0

2 changed files with 335 additions and 168 deletions

View File

@@ -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
* @version 1.2.0
* @since December 15, 2014
*/
public class NxtARControlProtocol {
@@ -53,69 +55,206 @@ 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
* listeners if needed.</p>
*
* @param blocking
* @return
* @return True if a message could be read, decoded and executed successfully. False otherwise.
* @throws IOException If reading the message fails. It is the same IOException
* as thrown by {@link DataInput#readByte()} if any.
*/
public boolean readAndExecuteMessage() throws IOException{
boolean success = false;
@@ -137,138 +276,7 @@ public class NxtARControlProtocol {
}
if(controlAction != null){
switch(controlAction.action){
case MOVE_BACKWARDS:
switch(controlAction.motor){
case MOTOR_A:
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_AB:
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_ABC:
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_AC:
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_B:
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_BC:
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_C:
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
break;
}
break;
case MOVE_FORWARD:
switch(controlAction.motor){
case MOTOR_A:
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_AB:
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_ABC:
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_AC:
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_B:
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_BC:
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_C:
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
break;
}
break;
case RECENTER:
switch(controlAction.motor){
case MOTOR_A:
recenterMotor(MotorPort.MOTOR_A);
break;
case MOTOR_AB:
recenterMotor(MotorPort.MOTOR_A);
recenterMotor(MotorPort.MOTOR_B);
break;
case MOTOR_ABC:
recenterMotor(MotorPort.MOTOR_A);
recenterMotor(MotorPort.MOTOR_B);
recenterMotor(MotorPort.MOTOR_C);
break;
case MOTOR_AC:
recenterMotor(MotorPort.MOTOR_A);
recenterMotor(MotorPort.MOTOR_C);
break;
case MOTOR_B:
recenterMotor(MotorPort.MOTOR_B);
break;
case MOTOR_BC:
recenterMotor(MotorPort.MOTOR_B);
recenterMotor(MotorPort.MOTOR_C);
break;
case MOTOR_C:
recenterMotor(MotorPort.MOTOR_C);
break;
}
break;
case STOP:
switch(controlAction.motor){
case MOTOR_A:
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_AB:
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_ABC:
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_AC:
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_B:
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_BC:
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_C:
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
break;
}
break;
case USER_1:
case USER_2:
case USER_3:
notifyListeners(controlAction.action, controlAction.motor, controlAction.speed);
success = true;
break;
}
success = executeControlAction(controlAction);
}
}
@@ -293,7 +301,7 @@ public class NxtARControlProtocol {
* @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{
public DecodedControlAction decodeMessage(final byte[] message) throws IllegalArgumentException{
Action action = Action.STOP;
Motor motor = Motor.MOTOR_ABC;
DecodedControlAction controlAction;
@@ -359,6 +367,161 @@ public class NxtARControlProtocol {
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){
case MOTOR_A:
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_AB:
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_ABC:
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_AC:
controlMotor(MotorPort.MOTOR_A, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_B:
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_BC:
controlMotor(MotorPort.MOTOR_B, MotorAction.BACKWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
break;
case MOTOR_C:
controlMotor(MotorPort.MOTOR_C, MotorAction.BACKWARD, controlAction.speed);
break;
}
success = true;
break;
case MOVE_FORWARD:
switch(controlAction.motor){
case MOTOR_A:
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_AB:
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_ABC:
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_AC:
controlMotor(MotorPort.MOTOR_A, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_B:
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_BC:
controlMotor(MotorPort.MOTOR_B, MotorAction.FORWARD, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
break;
case MOTOR_C:
controlMotor(MotorPort.MOTOR_C, MotorAction.FORWARD, controlAction.speed);
break;
}
success = true;
break;
case RECENTER:
switch(controlAction.motor){
case MOTOR_A:
recenterMotor(MotorPort.MOTOR_A);
break;
case MOTOR_AB:
recenterMotor(MotorPort.MOTOR_A);
recenterMotor(MotorPort.MOTOR_B);
break;
case MOTOR_ABC:
recenterMotor(MotorPort.MOTOR_A);
recenterMotor(MotorPort.MOTOR_B);
recenterMotor(MotorPort.MOTOR_C);
break;
case MOTOR_AC:
recenterMotor(MotorPort.MOTOR_A);
recenterMotor(MotorPort.MOTOR_C);
break;
case MOTOR_B:
recenterMotor(MotorPort.MOTOR_B);
break;
case MOTOR_BC:
recenterMotor(MotorPort.MOTOR_B);
recenterMotor(MotorPort.MOTOR_C);
break;
case MOTOR_C:
recenterMotor(MotorPort.MOTOR_C);
break;
}
success = true;
break;
case STOP:
switch(controlAction.motor){
case MOTOR_A:
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_AB:
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_ABC:
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_AC:
controlMotor(MotorPort.MOTOR_A, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_B:
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_BC:
controlMotor(MotorPort.MOTOR_B, MotorAction.STOP, controlAction.speed);
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
break;
case MOTOR_C:
controlMotor(MotorPort.MOTOR_C, MotorAction.STOP, controlAction.speed);
break;
}
success = true;
break;
case USER_1:
case USER_2:
case USER_3:
notifyListeners(controlAction.action, controlAction.motor, controlAction.speed);
success = true;
break;
}
}
return success;
}
/**
* <p>Adds an {@link UserActionListener} to this object's listeners list calling it's
* {@link UserActionListener#onListenerRegistered()} method. Adding a listener that
@@ -367,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.");
@@ -385,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.");
@@ -402,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)
@@ -512,4 +675,8 @@ public class NxtARControlProtocol {
break;
}
}
private int clamp(int i, int min, int max){
return i > max ? max : (i < min ? min : i);
}
}

View File

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