First commit. Started working with the subsumption API.

.classpath

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="con" path="org.lejos.nxt.ldt.LEJOS_LIBRARY_CONTAINER/nxt"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>

.gitignore

@@ -0,0 +1,5 @@
+*~
+*.class
+bin/
+*.nxd
+*.nxj

.project

@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>RyABI</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.lejos.nxt.ldt.leJOSBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.lejos.nxt.ldt.leJOSNature</nature>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

LICENSE

@@ -0,0 +1,22 @@
+Copyright (c) 2016, Miguel Angel Astor Romero
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.org

@@ -0,0 +1,7 @@
+* RyABI
+
+** Abstract
+RyABI (/Robótica y Aprendizaje Bio-Inspirado/ - Robotics and Bio-Inspired Learning) is a control program for LEGO Mindstorms NXT using the LeJOS NXJ
+firmware. It is developed as part of the homonimous course being taught at the Computer Sciences School of the UCV.
+
+Future verions of this README file will include a technical report on the development of the project.

src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java

@@ -0,0 +1,43 @@
+package ve.ucv.ciens.cicore.icaro.ryabi;
+
+import lejos.nxt.Button;
+import lejos.nxt.LightSensor;
+import lejos.nxt.SensorPort;
+import lejos.nxt.TouchSensor;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.subsumption.Arbitrator;
+import lejos.robotics.subsumption.Behavior;
+import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.SensorCalibrationBehavior;
+import ve.ucv.ciens.cicore.icaro.ryabi.utils.QuitButtonListener;
+
+public class RyABI {
+	private static final float WHEEL_RADIUS = 56.0f;
+	private static final float TRACK_WIDTH = 155.0f;
+
+	private static UltrasonicSensor sonar;
+	private static TouchSensor      touch;
+	private static CompassHTSensor  compass;
+	private static LightSensor      light;
+	private static Behavior[]       behaviors;
+	private static Arbitrator       arbitrator;
+
+	public static void main(String[] args) {
+		/* Create the sensors. */
+		sonar = new UltrasonicSensor(SensorPort.S1);
+		touch = new TouchSensor(SensorPort.S2);
+		compass = new CompassHTSensor(SensorPort.S3);
+		light = new LightSensor(SensorPort.S4);
+
+		/* Register escape button for forced exit. */
+		Button.ESCAPE.addButtonListener(new QuitButtonListener());
+
+		/* Create the behaviors. */
+		behaviors = new Behavior[1];
+		behaviors[0] = new SensorCalibrationBehavior(sonar, touch, light, compass, WHEEL_RADIUS, TRACK_WIDTH);
+
+		/* Start the program. */
+		arbitrator = new Arbitrator(behaviors, true);
+		arbitrator.start();
+	}
+}

src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/BaseBehavior.java

@@ -0,0 +1,30 @@
+package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
+
+import lejos.nxt.LightSensor;
+import lejos.nxt.TouchSensor;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.subsumption.Behavior;
+
+public abstract class BaseBehavior implements Behavior {
+	protected UltrasonicSensor         sonar;
+	protected TouchSensor              touch;
+	protected CompassHTSensor          compass;
+	protected LightSensor              light;
+
+	public BaseBehavior(UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass) {
+		this.sonar = sonar;
+		this.touch = touch;
+		this.compass = compass;
+		this.light = light;
+	}
+
+	@Override
+	public abstract boolean takeControl();
+
+	@Override
+	public abstract void action();
+
+	@Override
+	public abstract void suppress();
+}

src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/GetToBorderBehavior.java

@@ -0,0 +1,60 @@
+package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
+
+import lejos.nxt.LightSensor;
+import lejos.nxt.Motor;
+import lejos.nxt.TouchSensor;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.navigation.ArcRotateMoveController;
+import lejos.robotics.navigation.CompassPilot;
+import lejos.robotics.objectdetection.RangeFeatureDetector;
+import lejos.robotics.objectdetection.TouchFeatureDetector;
+import ve.ucv.ciens.cicore.icaro.ryabi.detectors.FeatureDetectionListener;
+import ve.ucv.ciens.cicore.icaro.ryabi.detectors.LightFeatureDetector;
+
+@SuppressWarnings("deprecation")
+public class GetToBorderBehavior extends BaseBehavior {
+	private ArcRotateMoveController  pilot;
+	private RangeFeatureDetector     rangeDetector;
+	private TouchFeatureDetector     touchDetector;
+	private LightFeatureDetector     lightDetector;
+	private FeatureDetectionListener featureListener;
+
+	public GetToBorderBehavior(UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass) {
+		super(sonar, touch, light, compass);
+
+		pilot = new CompassPilot(compass, 56f, 155f, Motor.A, Motor.C);
+
+		rangeDetector = new RangeFeatureDetector(sonar, 20, 200);
+		touchDetector = new TouchFeatureDetector(touch);
+		lightDetector = new LightFeatureDetector(light);
+
+		rangeDetector.enableDetection(true);
+		touchDetector.enableDetection(true);
+		lightDetector.enableDetection(true);
+
+		featureListener = new FeatureDetectionListener(pilot, compass);
+		rangeDetector.addListener(featureListener);
+		touchDetector.addListener(featureListener);
+		lightDetector.addListener(featureListener);
+	}
+
+	@Override
+	public boolean takeControl() {
+		return false;
+	}
+
+	@Override
+	public void action() {
+		rangeDetector.enableDetection(true);
+		touchDetector.enableDetection(true);
+		lightDetector.enableDetection(true);
+	}
+
+	@Override
+	public void suppress() {
+		rangeDetector.enableDetection(false);
+		touchDetector.enableDetection(false);
+		lightDetector.enableDetection(false);
+	}
+}

src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java

@@ -0,0 +1,51 @@
+package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
+
+import lejos.nxt.Button;
+import lejos.nxt.LightSensor;
+import lejos.nxt.Motor;
+import lejos.nxt.TouchSensor;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.navigation.DifferentialPilot;
+
+public class SensorCalibrationBehavior extends BaseBehavior {
+	private boolean sensorsCalibrated;
+	private float wheelRadius;
+	private float trackWidth;
+
+	public SensorCalibrationBehavior(UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) {
+		super(sonar, touch, light, compass);
+		sensorsCalibrated = false;
+		this.wheelRadius = wheelRadius;
+		this.trackWidth = trackWidth;
+	}
+
+	@Override
+	public boolean takeControl() {
+		return !sensorsCalibrated;
+	}
+
+	@Override
+	public void action() {
+		System.out.println("Calib. compass");
+		DifferentialPilot p = new DifferentialPilot(wheelRadius, trackWidth, Motor.A, Motor.C);
+		p.setRotateSpeed(25);
+		compass.startCalibration();
+		p.rotate(720, false);
+		compass.stopCalibration();
+
+		System.out.println("Calib. light s.");
+		light.setFloodlight(true);
+		System.out.println("Point at dark\nand press ENTER");
+		Button.ENTER.waitForPress();
+		light.calibrateLow();
+		System.out.println("Point at light\nand press ENTER");
+		Button.ENTER.waitForPress();
+		light.calibrateHigh();
+
+		sensorsCalibrated = true;
+	}
+
+	@Override
+	public void suppress() { }
+}

src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/FeatureDetectionListener.java

@@ -0,0 +1,57 @@
+package ve.ucv.ciens.cicore.icaro.ryabi.detectors;
+
+import lejos.nxt.Sound;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.navigation.ArcRotateMoveController;
+import lejos.robotics.objectdetection.Feature;
+import lejos.robotics.objectdetection.FeatureDetector;
+import lejos.robotics.objectdetection.FeatureListener;
+import lejos.robotics.objectdetection.RangeFeatureDetector;
+import lejos.robotics.objectdetection.TouchFeatureDetector;
+
+public class FeatureDetectionListener implements FeatureListener {
+	private ArcRotateMoveController pilot;
+	private CompassHTSensor         compass;
+
+	public FeatureDetectionListener(ArcRotateMoveController pilot, CompassHTSensor compass) {
+		this.pilot = pilot;
+		this.compass = compass;
+	}
+
+	@Override
+	public void featureDetected(Feature feature, FeatureDetector detector) {
+		if(detector instanceof TouchFeatureDetector) {
+			
+
+		} else if(detector instanceof LightFeatureDetector) {
+			// TODO: Add light sensor handling here.
+
+		}else if(detector instanceof RangeFeatureDetector) {
+			// TODO: Add sonar handling here.
+			Sound.beep();
+
+			detector.enableDetection(false);
+			pilot.stop();
+			pilot.travel(-100);
+			rotate90();
+			pilot.forward();
+			detector.enableDetection(true);
+		}
+	}
+
+	private void rotate90() {
+		float cMeasure;
+
+		compass.resetCartesianZero();
+
+		pilot.setRotateSpeed(25);
+		pilot.rotate(3000, true);
+
+		cMeasure = 360.0f;
+		try { Thread.sleep(200); } catch (InterruptedException e) { }
+		while(cMeasure > 285.0f) {
+			cMeasure = compass.getDegreesCartesian();
+		}
+		pilot.stop();
+	}
+}

src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/LightFeatureDetector.java

@@ -0,0 +1,76 @@
+package ve.ucv.ciens.cicore.icaro.ryabi.detectors;
+
+import lejos.geom.Point;
+import lejos.nxt.LightSensor;
+import lejos.robotics.RangeReading;
+import lejos.robotics.objectdetection.Feature;
+import lejos.robotics.objectdetection.FeatureDetectorAdapter;
+import lejos.robotics.objectdetection.RangeFeature;
+
+public class LightFeatureDetector extends FeatureDetectorAdapter {
+	private static final int DELAY = 50;
+
+	private LightSensor lightSensor;
+	private int threshold;
+	private boolean invert;
+	private float angle = 0;
+	private float range = 0;
+
+	public LightFeatureDetector(LightSensor lightSensor) {
+		this(lightSensor, 50, false, 0, 0);
+	}
+
+	public LightFeatureDetector(LightSensor lightSensor, int threshold) {
+		this(lightSensor, threshold, false, 0, 0);
+	}
+
+	public LightFeatureDetector(LightSensor lightSensor, boolean invert) {
+		this(lightSensor, 50, invert, 0, 0);
+	}
+
+	public LightFeatureDetector(LightSensor lightSensor, double xOffset, double yOffset) {
+		this(lightSensor, 50, false, xOffset, yOffset);
+	}
+
+	public LightFeatureDetector(LightSensor lightSensor, int threshold, double xOffset, double yOffset) {
+		this(lightSensor, threshold, false, xOffset, yOffset);
+	}
+
+	public LightFeatureDetector(LightSensor lightSensor, boolean invert, double xOffset, double yOffset) {
+		this(lightSensor, 50, invert, xOffset, yOffset);
+	}
+
+	public LightFeatureDetector(LightSensor lightSensor, int threshold, boolean invert, double xOffset, double yOffset) {
+		super(DELAY);
+		this.lightSensor = lightSensor;
+		this.threshold = threshold;
+		this.invert = invert;
+
+		// Calculate angle a distance of bumper from center:
+		Point robot_center = new Point(0, 0);
+		Point bumper_p = new Point((float)xOffset, (float)yOffset);
+		range = (float)robot_center.distance(xOffset, yOffset);
+		angle = robot_center.angleTo(bumper_p) - 90;
+	}
+
+	@Override
+	public Feature scan() {
+		RangeFeature rf = null;
+		if(lightSensor.getLightValue() <= threshold) {
+			RangeReading rr = new RangeReading(angle, range);
+			rf = new RangeFeature(rr);
+		}
+		return rf;
+	}
+
+	@Override
+	protected void notifyListeners(Feature feature) {
+		super.notifyListeners(feature);
+
+		if(invert) {
+			while(lightSensor.getLightValue() >= threshold);
+		} else {
+			while(lightSensor.getLightValue() <= threshold);
+		}
+	}
+}

src/ve/ucv/ciens/cicore/icaro/ryabi/threads/CompassThread.java

@@ -0,0 +1,29 @@
+package ve.ucv.ciens.cicore.icaro.ryabi.threads;
+
+import lejos.nxt.LCD;
+import lejos.nxt.addon.CompassHTSensor;
+
+public class CompassThread extends Thread {
+	CompassHTSensor compass;
+	boolean done;
+
+	public CompassThread(CompassHTSensor compass) {
+		this.compass = compass;
+		done = false;
+	}
+
+	public synchronized void end() {
+		done = true;
+	}
+
+	public void run() {
+		while(!done) {
+			LCD.clear();
+			System.out.println("C (DEG): " + compass.getDegrees());
+			System.out.println("C (CRT): " + compass.getDegreesCartesian());
+			try {
+				Thread.sleep(1000);
+			} catch(InterruptedException i) { }
+		}
+	}
+}

src/ve/ucv/ciens/cicore/icaro/ryabi/utils/QuitButtonListener.java

@@ -0,0 +1,49 @@
+package ve.ucv.ciens.cicore.icaro.ryabi.utils;
+
+import lejos.nxt.Button;
+import lejos.nxt.ButtonListener;
+
+public class QuitButtonListener implements ButtonListener {
+	TimeCounter counter;
+
+	@Override
+	public void buttonPressed(Button b) {
+		counter = new TimeCounter();
+		counter.start();
+	}
+
+	@Override
+	public void buttonReleased(Button b) {
+		if(counter != null) {
+			counter.finish();
+			counter = null;
+		}
+	}
+
+	class TimeCounter extends Thread {
+		private boolean done;
+		private long timeMilisBefore;
+
+		public TimeCounter() {
+			done = false;
+		}
+
+		@Override
+		public void run() {
+			long timeMilisNow;
+
+			timeMilisBefore = System.currentTimeMillis();
+			while(!done) {
+				try { Thread.sleep(100); } catch (InterruptedException e) { }
+				timeMilisNow = System.currentTimeMillis();
+
+				if(timeMilisNow - timeMilisBefore > 3000)
+					System.exit(0);
+			}
+		}
+
+		public void finish() {
+			done = true;
+		}
+	}
+}