Merge branch 'develop'

src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java

@@ -18,29 +18,19 @@ package ve.ucv.ciens.ccg.nxtarbot.threads;
 import java.io.DataInputStream;
 import java.io.IOException;
 
-import lejos.nxt.BasicMotorPort;
-import lejos.nxt.Battery;
 import lejos.nxt.Motor;
-import ve.ucv.ciens.ccg.nxtarbot.protocol.MotorMasks;
+import ve.ucv.ciens.icaro.libnxtarcontrol.DecodedControlAction;
+import ve.ucv.ciens.icaro.libnxtarcontrol.NxtARControlProtocol;
+import ve.ucv.ciens.icaro.libnxtarcontrol.UserActionListener;
 
 /**
- * <p>Class to control the motors on the robot based on isntructions received via
+ * <p>Class to control the motors on the robot based on instructions received via
  * Bluetooth.</p>
- * 
- * <p>The instructions are codified on a very simple protocol:<p>
- * <ul>
- *     <li> Every protocol packet is exactly two bytes long.</li>
- *     <li> The lowest order byte codifies the motor to control and the rotation
- *          direction of the same on it's highest four bits. The lower four bits
- *          indicate that the central motor should return to it's position of
- *          origin. This four bits can also be used for future expansions.
- *     <li> The highest order byte, a number between 0 and 100, contains the
- *          power to send to the motor.</li> 
- * </ul>
  */
-public class MotorControlThread extends Thread{
+public class MotorControlThread extends Thread implements UserActionListener{
 	private boolean done;
-	private DataInputStream inputStream;
+	private final DataInputStream inputStream;
+	private final NxtARControlProtocol controlProtocol;
 
 	/**
 	 * Create a new MotorControlThread.
@@ -48,90 +38,59 @@ public class MotorControlThread extends Thread{
 	 * @param inputStream The stream to receive motor commands from.
 	 * @throws NullPointerException If inputStream is null.
 	 */
-	public MotorControlThread(DataInputStream inputStream) throws NullPointerException{
+	public MotorControlThread(final DataInputStream inputStream) throws NullPointerException{
 		if(inputStream == null)
 			throw new NullPointerException("Input stream is null.");
 
-		done = false;
+		this.done = false;
 		this.inputStream = inputStream;
+		this.controlProtocol = new NxtARControlProtocol(this.inputStream);
+		this.controlProtocol.registerUserActionListener(this);
 	}
 
 	/**
 	 * <p>Marks this thread as ready to finish cleanly.</p>
 	 */
 	public void finish(){
-		done = true;
+		this.done = true;
+		this.controlProtocol.removeUserActionListener(this);
 	}
 
 	/**
-	 * <p>Receive and process motor control instructions via Bluetooth.</p>
+	 * <p>Receive and process motor control instructions using the protocol library.</p>
 	 */
 	@Override
 	public void run(){
-		boolean motorA, motorB, motorC, recenterMotorB, rotate90;
-		int direction, rotation, tacho;
-		byte[] message = new byte[2];
-
 		while(!done){
 			try{
-				// Read the two bytes indicated by the protocol.
-				message[0] = inputStream.readByte();
-				message[1] = inputStream.readByte();
-
-				// Decode the instruction parameters.
-				rotate90       = (message[0] & MotorMasks.ROTATE_90) > 0 ? true : false;
-				recenterMotorB = (message[0] & MotorMasks.RECENTER) > 0 ? true : false;
-				motorA         = (message[0] & MotorMasks.MOTOR_A) > 0 ? true : false;
-				motorB         = (message[0] & MotorMasks.MOTOR_B) > 0 ? true : false;
-				motorC         = (message[0] & MotorMasks.MOTOR_C) > 0 ? true : false;
-				direction      = (message[0] & MotorMasks.DIRECTION) > 0 ? BasicMotorPort.FORWARD : BasicMotorPort.BACKWARD;
-
-				if(motorA){
-					// Set motor A to run at specified speed.
-					Motor.A.setSpeed(message[1] * Battery.getVoltage());
-					if(direction == BasicMotorPort.FORWARD)
-						Motor.A.forward();
-					else if(direction == BasicMotorPort.BACKWARD)
-						Motor.A.backward();
-				}
-
-				if(motorB){
-					// Set motor B to run at specified speed.
-					Motor.B.setSpeed(message[1] * Battery.getVoltage());
-					if(direction == BasicMotorPort.FORWARD)
-						Motor.B.forward();
-					else if(direction == BasicMotorPort.BACKWARD)
-						Motor.B.backward();
-				}
-
-				if(motorC){
-					// Set motor C to run at specified speed.
-					Motor.C.setSpeed(message[1] * Battery.getVoltage());
-					if(direction == BasicMotorPort.FORWARD)
-						Motor.C.forward();
-					else if(direction == BasicMotorPort.BACKWARD)
-						Motor.C.backward();
-				}
-
-				if(recenterMotorB){
-					// Return motor B to it's origin.
-					System.out.println("RECENTER");
-					Motor.B.setSpeed(50 * Battery.getVoltage());
-					tacho = Motor.B.getTachoCount() % 360;
-					rotation = -(tacho);
-					Motor.B.rotate(rotation, false);
-				}
-
-				if(rotate90){
-					// Rotate 90 degrees.
-					System.out.println("ROTATE 90");
-					Motor.B.rotate(-120, false);
-				}
-
+				controlProtocol.readAndExecuteMessage();
 			}catch(IOException io){
 				// On disconnection, terminate.
 				done = true;
 			}
 		}
 	}
+
+	@Override
+	public void onListenerRegistered() {
+		System.out.println("Registered.");
+	}
+
+	@Override
+	public void onUserAction1(DecodedControlAction.Motor motorFlag, int speed) {
+		// Rotate 90 degrees.
+		System.out.println("USER_1 :: ROTATE 90");
+		Motor.B.rotate(-120, false);
+	}
+
+	@Override
+	public void onUserAction2(DecodedControlAction.Motor motorFlag, int speed) { }
+
+	@Override
+	public void onUserAction3(DecodedControlAction.Motor motorFlag, int speed) { }
+
+	@Override
+	public void onListenerRemoved() {
+		System.out.println("Remove.");
+	}
 }