WallyHackenslacker/RyABI
1
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

First commit. Started working with the subsumption API.

Browse Source
This commit is contained in:
Wally Hackenslacker
2016-05-17 21:29:38 -04:00
commit 56d327d2e4
13 changed files with 458 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
.classpath Normal file
View File

@@ -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>

5
.gitignore vendored Normal file
View File

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

23
.project Normal file
View File

@@ -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>

22
LICENSE Normal file
View File

@@ -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.

7
README.org Normal file
View File

@@ -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.

43
src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java Normal file
View File

@@ -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();
}
}

30
src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/BaseBehavior.java Normal file
View File

@@ -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();
}

60
src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/GetToBorderBehavior.java Normal file
View File

@@ -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);
}
}

51
src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java Normal file
View File

@@ -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() { }
}

57
src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/FeatureDetectionListener.java Normal file
View File

@@ -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();
}
}

76
src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/LightFeatureDetector.java Normal file
View File

@@ -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);
}
}
}

29
src/ve/ucv/ciens/cicore/icaro/ryabi/threads/CompassThread.java Normal file
View File

@@ -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) { }
}
}
}

49
src/ve/ucv/ciens/cicore/icaro/ryabi/utils/QuitButtonListener.java Normal file
View File

@@ -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;
}
}
}