Merge branch 'develop'

README.md

@@ -1,27 +1,4 @@
-NxtAR: A generic software architecture for Augmented Reality based mobile robot control.
+NxtAR-bot
-========================================================================================
+=========
 
-Robot control module
+Modulo 3 de mi trabajo especial de grado.
---------------------
-
-### Abstract ###
-
-NxtAR is a generic software architecture for the development of Augmented Reality games
-and applications centered around mobile robot control. This is a reference implementation
-with support for [LEGO Mindstorms NXT][1] mobile robots.
-
-### Module description ###
-
-The robot control module is a control application that receives control commands sent from the 
-[NxtAR-cam][2] module and executes concrete control instructions as supported by the [LejOS][3] firmware.
-
-### Module installation and usage. ###
-
-Upload the NxtAR-bot_XXXXXX.nxj file to the LEGO Mindstorms NXT robot and execute it. Then follow the
-on-screen instructions. More detailed usage instructions can be found in the [NxtAR Android backend module][4]
-documentation. The robot must have the LejOS firmware installed prior to loading the nxj file.
-
- [1]: http://www.lego.com/en-us/mindstorms/?domainredir=mindstorms.lego.com
- [2]: https://github.com/sagge-miky/NxtAR-cam
- [3]: http://www.lejos.org/nxj.php
- [4]: https://github.com/sagge-miky/NxtAR-android

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

@@ -18,19 +18,29 @@ 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.icaro.libnxtarcontrol.DecodedControlAction;
+import ve.ucv.ciens.ccg.nxtarbot.protocol.MotorMasks;
-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 instructions received via
+ * <p>Class to control the motors on the robot based on isntructions 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 implements UserActionListener{
+public class MotorControlThread extends Thread{
 	private boolean done;
-	private final DataInputStream inputStream;
+	private DataInputStream inputStream;
-	private final NxtARControlProtocol controlProtocol;
 
 	/**
 	 * Create a new MotorControlThread.
@@ -38,59 +48,90 @@ public class MotorControlThread extends Thread implements UserActionListener{
 	 * @param inputStream The stream to receive motor commands from.
 	 * @throws NullPointerException If inputStream is null.
 	 */
-	public MotorControlThread(final DataInputStream inputStream) throws NullPointerException{
+	public MotorControlThread(DataInputStream inputStream) throws NullPointerException{
 		if(inputStream == null)
 			throw new NullPointerException("Input stream is null.");
 
-		this.done = false;
+		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(){
-		this.done = true;
+		done = true;
-		this.controlProtocol.removeUserActionListener(this);
 	}
 
 	/**
-	 * <p>Receive and process motor control instructions using the protocol library.</p>
+	 * <p>Receive and process motor control instructions via Bluetooth.</p>
 	 */
 	@Override
 	public void run(){
+		boolean motorA, motorB, motorC, recenterMotorB, rotate90;
+		int direction, rotation, tacho;
+		byte[] message = new byte[2];
+
 		while(!done){
 			try{
-				controlProtocol.readAndExecuteMessage();
+				// 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);
+				}
+
 			}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.");
-	}
 }