From a2ca1382e655fb77866c193ab8d1ab964c6427bc Mon Sep 17 00:00:00 2001
From: unknown <sonofgrendel@gmail.com>
Date: Mon, 23 Jun 2014 16:07:58 -0430
Subject: [PATCH] Added support for automatic actions.

---
 .../ccg/nxtarbot/protocol/MotorMasks.java     |  4 ++--
 .../nxtarbot/threads/MotorControlThread.java  | 21 ++++++++++++-------
 2 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java b/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java
index 2239da9..1d9d86a 100644
--- a/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java
+++ b/src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java
@@ -19,7 +19,7 @@ package ve.ucv.ciens.ccg.nxtarbot.protocol;
  * <p>Bit masks used to code/decode the control instructions sent by NxtAR-cam to
  * NxtAR-bot.</p>
  *
- * <p>Expansions 1-3 are currently unused.</p>
+ * <p>Expansions 2-3 are currently unused.</p>
  */
 public abstract class MotorMasks {
 	public static final byte MOTOR_A     = (byte)0x01;
@@ -27,7 +27,7 @@ public abstract class MotorMasks {
 	public static final byte MOTOR_C     = (byte)0x04;
 	public static final byte DIRECTION   = (byte)0x08;
 	public static final byte RECENTER    = (byte)0x10;
-	public static final byte EXPANSION_1 = (byte)0x20;
+	public static final byte ROTATE_90   = (byte)0x20;
 	public static final byte EXPANSION_2 = (byte)0x40;
 	public static final byte EXPANSION_3 = (byte)0x80;
 }
diff --git a/src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java
index a6e4503..2129250 100644
--- a/src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java
+++ b/src/ve/ucv/ciens/ccg/nxtarbot/threads/MotorControlThread.java
@@ -68,7 +68,7 @@ public class MotorControlThread extends Thread{
 	 */
 	@Override
 	public void run(){
-		boolean motorA, motorB, motorC, recenterMotorB;
+		boolean motorA, motorB, motorC, recenterMotorB, rotate90;
 		int direction, rotation, tacho;
 		byte[] message = new byte[2];
 
@@ -79,11 +79,12 @@ public class MotorControlThread extends Thread{
 				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;
+				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.
@@ -113,16 +114,22 @@ public class MotorControlThread extends Thread{
 				}
 
 				if(recenterMotorB){
-					System.out.println("RECENTER");
 					// 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(90, false);
+				}
+
 			}catch(IOException io){
-				// On disconnection terminate.
+				// On disconnection, terminate.
 				done = true;
 			}
 		}