Added light sensor calibration and motor control.

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

@@ -19,37 +19,28 @@ 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.CommRecv;
+import ve.ucv.ciens.ccg.nxtarbot.threads.MotorControlThread;
-import ve.ucv.ciens.ccg.nxtarbot.threads.CommSend;
+import ve.ucv.ciens.ccg.nxtarbot.threads.SensorReportThread;
 
 public class NxtAR_bot{
 	private static DataOutputStream dataOutputStream;
 	private static DataInputStream dataInputStream;
 	private static NXTConnection bluetoothConnection;
-	private static CommRecv recv;
+	private static MotorControlThread recvThread;
-	private static CommSend send;
+	private static SensorReportThread sendThread;
 
-	public static void main(String[] args){
+	private static void quit(){
-		bluetoothConnection = Bluetooth.waitForConnection();
+		if(recvThread != null) recvThread.finish();
-		bluetoothConnection.setIOMode(NXTConnection.RAW);
+		if(sendThread != null) sendThread.finish();
-		dataOutputStream = bluetoothConnection.openDataOutputStream();
-		dataInputStream = bluetoothConnection.openDataInputStream();
-
-		System.out.println("Connected");
-
-		send = new CommSend(dataOutputStream);
-		recv = new CommRecv(dataInputStream);
-
-		recv.start();
-		send.start();
-
-		try{
-			recv.join();
-			send.join();
-		}catch(InterruptedException i){ }
 
+		if(bluetoothConnection != null){
 			try{
 				dataOutputStream.close();
 				dataInputStream.close();
@@ -58,4 +49,56 @@ public class NxtAR_bot{
 			}
 			bluetoothConnection.close();
 		}
+	}
+
+	public static void main(String[] args){
+		Motor.A.resetTachoCount();
+		Motor.B.resetTachoCount();
+		Motor.C.resetTachoCount();
+
+		LightSensor lightSensor = new LightSensor(SensorPort.S1);
+		lightSensor.setFloodlight(false);
+
+		System.out.println("Point at dark and press ENTER");
+		Button.ENTER.waitForPress();
+		lightSensor.calibrateLow();
+		System.out.println("--/--");
+
+		System.out.println("Point at light and press ENTER");
+		Button.ENTER.waitForPress();
+		lightSensor.calibrateHigh();
+		System.out.println("--/--");
+
+		Button.ESCAPE.addButtonListener(new QuitButtonListener());
+
+		bluetoothConnection = Bluetooth.waitForConnection();
+		bluetoothConnection.setIOMode(NXTConnection.RAW);
+		dataOutputStream = bluetoothConnection.openDataOutputStream();
+		dataInputStream = bluetoothConnection.openDataInputStream();
+
+		System.out.println("Connected");
+
+		sendThread = new SensorReportThread(dataOutputStream, lightSensor);
+		recvThread = new MotorControlThread(dataInputStream);
+
+		recvThread.start();
+		sendThread.start();
+
+		try{
+			recvThread.join();
+			sendThread.join();
+		}catch(InterruptedException i){ }
+
+		quit();
+	}
+
+	private static class QuitButtonListener implements ButtonListener{
+		@Override
+		public void buttonPressed(Button b) {
+			quit();
+		}
+
+		@Override
+		public void buttonReleased(Button b){ }
+	}
 }

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

@@ -13,30 +13,12 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-package ve.ucv.ciens.ccg.nxtarbot.threads;
+package ve.ucv.ciens.ccg.nxtarbot.protocol;
 
-import java.io.DataInputStream;
+public abstract class MotorMasks {
-import java.io.IOException;
+	public static final byte MOTOR_A   = (byte)0x01;
-
+	public static final byte MOTOR_B   = (byte)0x02;
-public class CommRecv extends Thread {
+	public static final byte MOTOR_C   = (byte)0x04;
-	private boolean done;
+	public static final byte DIRECTION = (byte)0x08;
-	private byte msg;
+	public static final byte RECENTER  = (byte)0xF0;
-	private DataInputStream iStream;
-
-	public CommRecv(DataInputStream iStream){
-		done = false;
-		this.iStream = iStream;
-	}
-
-	@Override
-	public void run(){
-		while(!done){
-			try{
-				msg = iStream.readByte();
-				System.out.println("Byte: " + Byte.toString(msg));
-			}catch(IOException io){
-				done = true;
-			}
-		}
-	}
 }

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

@@ -0,0 +1,92 @@
+/*
+ * 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;
+
+public class MotorControlThread extends Thread{
+	private boolean done;
+	private DataInputStream inputStream;
+
+	public MotorControlThread(DataInputStream inputStream){
+		done = false;
+		this.inputStream = inputStream;
+	}
+
+	public void finish(){
+		done = true;
+	}
+
+	@Override
+	public void run(){
+		boolean motorA, motorB, motorC, recenterMotorB;
+		int direction, rotation, tacho;
+		byte[] message = new byte[2];
+
+		while(!done){
+			try{
+				message[0] = inputStream.readByte();
+				message[1] = inputStream.readByte();
+
+				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){
+					Motor.A.setSpeed(message[1] * Battery.getVoltage());
+					if(direction == BasicMotorPort.FORWARD)
+						Motor.A.forward();
+					else if(direction == BasicMotorPort.BACKWARD)
+						Motor.A.backward();
+				}
+
+				if(motorB){
+					Motor.B.setSpeed(message[1] * Battery.getVoltage());
+					if(direction == BasicMotorPort.FORWARD)
+						Motor.B.forward();
+					else if(direction == BasicMotorPort.BACKWARD)
+						Motor.B.backward();
+				}
+
+				if(motorC){
+					Motor.C.setSpeed(message[1] * Battery.getVoltage());
+					if(direction == BasicMotorPort.FORWARD)
+						Motor.C.forward();
+					else if(direction == BasicMotorPort.BACKWARD)
+						Motor.C.backward();
+				}
+
+				if(recenterMotorB){
+					tacho = Motor.B.getTachoCount() % 360;
+					rotation = tacho > 180 ? 360 - tacho : -(tacho);
+					Motor.B.rotate(rotation);
+					Motor.B.resetTachoCount();
+				}
+
+			}catch(IOException io){
+				done = true;
+			}
+		}
+	}
+}

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

@@ -19,29 +19,32 @@ import java.io.DataOutputStream;
 import java.io.IOException;
 
 import lejos.nxt.LightSensor;
-import lejos.nxt.SensorPort;
 
-public class CommSend extends Thread {
+public class SensorReportThread extends Thread{
 	private boolean done;
-	private byte msg;
+	private LightSensor lightSensor;
-	private LightSensor light;
+	private DataOutputStream outputStream;
-	private DataOutputStream oStream;
 
-	public CommSend(DataOutputStream oStream){
+	public SensorReportThread(DataOutputStream outputStream, LightSensor lightSensor){
-		light = new LightSensor(SensorPort.S1);
+		this.lightSensor = lightSensor;
 		done = false;
-		msg = 0;
+		this.outputStream = outputStream;
-		this.oStream = oStream;
+	}
+
+	public void finish(){
+		done = true;
 	}
 
 	@Override
 	public void run(){
+		byte message = 0;
+
 		while(!done){
 			try{
-				if((msg = (byte)light.getLightValue()) < 40){
+				message = (byte)lightSensor.getLightValue();
-					oStream.writeByte(msg);
+				outputStream.writeByte(message);
-					oStream.flush();
+				outputStream.flush();
-				}
+
 			}catch(IOException io){
 				done = true;
 			}