commit 56d327d2e4914885a4bdb89a8f63e39d6085cf21 Author: Miguel Angel Date: Tue May 17 21:29:38 2016 -0400 First commit. Started working with the subsumption API. diff --git a/.classpath b/.classpath new file mode 100644 index 0000000..3dbabe6 --- /dev/null +++ b/.classpath @@ -0,0 +1,6 @@ + + + + + + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d9b197d --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +*~ +*.class +bin/ +*.nxd +*.nxj diff --git a/.project b/.project new file mode 100644 index 0000000..d5a5248 --- /dev/null +++ b/.project @@ -0,0 +1,23 @@ + + + RyABI + + + + + + org.eclipse.jdt.core.javabuilder + + + + + org.lejos.nxt.ldt.leJOSBuilder + + + + + + org.lejos.nxt.ldt.leJOSNature + org.eclipse.jdt.core.javanature + + diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..6414a84 --- /dev/null +++ b/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. diff --git a/README.org b/README.org new file mode 100644 index 0000000..2d4b907 --- /dev/null +++ b/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. diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java new file mode 100644 index 0000000..078f1d6 --- /dev/null +++ b/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(); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/BaseBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/BaseBehavior.java new file mode 100644 index 0000000..bb9b962 --- /dev/null +++ b/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(); +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/GetToBorderBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/GetToBorderBehavior.java new file mode 100644 index 0000000..990181b --- /dev/null +++ b/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); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java new file mode 100644 index 0000000..bb9f763 --- /dev/null +++ b/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() { } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/FeatureDetectionListener.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/FeatureDetectionListener.java new file mode 100644 index 0000000..222cf1d --- /dev/null +++ b/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(); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/LightFeatureDetector.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/LightFeatureDetector.java new file mode 100644 index 0000000..77fdad6 --- /dev/null +++ b/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); + } + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/threads/CompassThread.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/threads/CompassThread.java new file mode 100644 index 0000000..11bc8d4 --- /dev/null +++ b/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) { } + } + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/QuitButtonListener.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/QuitButtonListener.java new file mode 100644 index 0000000..edf482a --- /dev/null +++ b/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; + } + } +}