Merge branch 'develop'

src/ve/ucv/ciens/ccg/nxtarbot/NxtAR_bot.java

@@ -15,11 +15,122 @@
  */
 package ve.ucv.ciens.ccg.nxtarbot;
 
-import lejos.nxt.Button;
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
 
+import lejos.nxt.Button;
+import lejos.nxt.ButtonListener;
+import lejos.nxt.LightSensor;
+import lejos.nxt.Motor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.comm.Bluetooth;
+import lejos.nxt.comm.NXTConnection;
+import ve.ucv.ciens.ccg.nxtarbot.threads.MotorControlThread;
+import ve.ucv.ciens.ccg.nxtarbot.threads.SensorReportThread;
+
+/**
+ * Core class for the robot module of NxtAR.
+ */
 public class NxtAR_bot{
+	private static DataOutputStream dataOutputStream;
+	private static DataInputStream dataInputStream;
+	private static NXTConnection bluetoothConnection;
+	private static MotorControlThread recvThread;
+	private static SensorReportThread sendThread;
+
+	/**
+	 * <p>Finishes the communication threads anc closes the Bluetooth data streams, 
+	 * then quits the application.</p>
+	 */
+	private static void quit(){
+		if(recvThread != null) recvThread.finish();
+		if(sendThread != null) sendThread.finish();
+
+		if(bluetoothConnection != null){
+			try{
+				dataOutputStream.close();
+				dataInputStream.close();
+			}catch(IOException io){
+				System.out.println(io.getMessage());
+			}
+			bluetoothConnection.close();
+		}
+
+		System.exit(0);
+	}
+
+	/**
+	 * <p>Application entry point.</p>
+	 * <p>Resets the motors, calibrates the light sensor and starts the
+	 * networking threads.</p>
+	 * 
+	 * @param args Command line arguments.
+	 */
 	public static void main(String[] args){
-		System.out.println("Hello, world!");
-		Button.waitForAnyPress();
+		// Set a listener to force quit if the ESCAPE button is pressed.
+		Button.ESCAPE.addButtonListener(new QuitButtonListener());
+
+		// Reset the rotation counts of the motors.
+		Motor.A.resetTachoCount();
+		Motor.B.resetTachoCount();
+		Motor.C.resetTachoCount();
+
+		// Start the light sensor and calibrate it.
+		LightSensor lightSensor = new LightSensor(SensorPort.S1);
+		lightSensor.setFloodlight(false);
+
+		System.out.println("Point at dark\nand press ENTER");
+		Button.ENTER.waitForPress();
+		lightSensor.calibrateLow();
+		System.out.println("--/--");
+
+		System.out.println("Point at light\nand press ENTER");
+		Button.ENTER.waitForPress();
+		lightSensor.calibrateHigh();
+		System.out.println("--/--");
+
+		// Connect with a Bluetooth device in raw mode. Then get the connection
+		// streams.
+		bluetoothConnection = Bluetooth.waitForConnection();
+		bluetoothConnection.setIOMode(NXTConnection.RAW);
+		dataOutputStream = bluetoothConnection.openDataOutputStream();
+		dataInputStream = bluetoothConnection.openDataInputStream();
+
+		System.out.println("Connected");
+
+		// Start the networking threads and wait for them to finish. 
+		sendThread = new SensorReportThread(dataOutputStream, lightSensor);
+		recvThread = new MotorControlThread(dataInputStream);
+
+		recvThread.start();
+		sendThread.start();
+
+		try{
+			recvThread.join();
+			sendThread.join();
+		}catch(InterruptedException i){ }
+
+		quit();
+	}
+
+	/**
+	 * <p>Force quit button listener.</p>
+	 */
+	private static class QuitButtonListener implements ButtonListener{
+		/**
+		 * Force quit.
+		 */
+		@Override
+		public void buttonPressed(Button b){
+			System.exit(0);
+			//quit();
+		}
+
+		/**
+		 * Do nothing.
+		 */
+		@Override
+		public void buttonReleased(Button b){ }
 	}
 }

src/ve/ucv/ciens/ccg/nxtarbot/protocol/MotorMasks.java

@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2014 Miguel Angel Astor Romero
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+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>
+ */
+public abstract class MotorMasks {
+	public static final byte MOTOR_A     = (byte)0x01;
+	public static final byte MOTOR_B     = (byte)0x02;
+	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 EXPANSION_2 = (byte)0x20;
+	public static final byte EXPANSION_3 = (byte)0x20;
+}

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

@@ -0,0 +1,130 @@
+/*
+ * Copyright (C) 2014 Miguel Angel Astor Romero
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+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;
+
+/**
+ * <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{
+	private boolean done;
+	private DataInputStream inputStream;
+
+	/**
+	 * Create a new MotorControlThread.
+	 * 
+	 * @param inputStream The stream to receive motor commands from.
+	 * @throws NullPointerException If inputStream is null.
+	 */
+	public MotorControlThread(DataInputStream inputStream) throws NullPointerException{
+		if(inputStream == null)
+			throw new NullPointerException("Input stream is null.");
+
+		done = false;
+		this.inputStream = inputStream;
+	}
+
+	/**
+	 * <p>Marks this thread as ready to finish cleanly.</p>
+	 */
+	public void finish(){
+		done = true;
+	}
+
+	/**
+	 * <p>Receive and process motor control instructions via Bluetooth.</p>
+	 */
+	@Override
+	public void run(){
+		boolean motorA, motorB, motorC, recenterMotorB;
+		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.
+				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){
+					System.out.println("RECENTER");
+					// Return motor B to it's origin.
+					Motor.B.setSpeed(50 * Battery.getVoltage());
+					tacho = Motor.B.getTachoCount() % 360;
+					rotation = -(tacho);
+					Motor.B.rotate(rotation, false);
+				}
+
+			}catch(IOException io){
+				// On disconnection terminate.
+				done = true;
+			}
+		}
+	}
+}

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

@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2014 Miguel Angel Astor Romero
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package ve.ucv.ciens.ccg.nxtarbot.threads;
+
+import java.io.DataOutputStream;
+import java.io.IOException;
+
+import lejos.nxt.LightSensor;
+
+/**
+ * <p>Class to report the values read from the different sensors through a
+ * Bluetooth stream.</p>
+ */
+public class SensorReportThread extends Thread{
+	private boolean done;
+	private LightSensor lightSensor;
+	private DataOutputStream outputStream;
+
+	/**
+	 * <p>Create a new SensorReportThread.</p>
+	 * 
+	 * @param outputStream The stream to send the data to.
+	 * @param lightSensor An initialized and calibrated light sensor.
+	 */
+	public SensorReportThread(DataOutputStream outputStream, LightSensor lightSensor) throws NullPointerException{
+		if(outputStream == null)
+			throw new NullPointerException("Output stream is null.");
+
+		if(lightSensor == null)
+			throw new NullPointerException("Sensor is null.");
+
+		this.lightSensor = lightSensor;
+		done = false;
+		this.outputStream = outputStream;
+	}
+
+	/**
+	 * <p>Marks this thread as ready to finish cleanly.</p>
+	 */
+	public void finish(){
+		done = true;
+	}
+
+	/**
+	 * <p>Writes the values read from the light sensor to the output stream.</p>
+	 */
+	@Override
+	public void run(){
+		byte message = 0;
+
+		while(!done){
+			try{
+				message = (byte)lightSensor.getLightValue();
+				outputStream.writeByte(message);
+				outputStream.flush();
+			}catch(IOException io){
+				// On disconnection terminate.
+				done = true;
+			}
+		}
+	}
+}