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

Wander and collision avoidance behaviors.

Browse Source
This commit is contained in:
Miguel Angel Astor Romero
2016-05-20 16:49:43 -04:30
parent 56d327d2e4
commit 283baef6b7
13 changed files with 307 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java
View File

@@ -2,39 +2,73 @@ package ve.ucv.ciens.cicore.icaro.ryabi;
import lejos.nxt.Button;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
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 lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.AvoidObstaclesBehavior;
import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.WanderBehavior;
import ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectionListener;
import ve.ucv.ciens.cicore.icaro.ryabi.sensors.LightFeatureDetector;
import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.SensorCalibrationBehavior;
import ve.ucv.ciens.cicore.icaro.ryabi.utils.QuitButtonListener;
@SuppressWarnings("deprecation")
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;
private static ArcRotateMoveController pilot;
private static UltrasonicSensor sonar;
private static TouchSensor touch;
private static CompassHTSensor compass;
private static LightSensor light;
private static Behavior[] behaviors;
private static Arbitrator arbitrator;
private static RangeFeatureDetector rangeDetector;
private static TouchFeatureDetector touchDetector;
private static LightFeatureDetector lightDetector;
private static FeatureDetectionListener featureListener;
public static void main(String[] args) {
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);
/* Create the pilot. */
pilot = new CompassPilot(compass, WHEEL_RADIUS, TRACK_WIDTH, Motor.A, Motor.C);
/* Create the feature detectors and their listener. */
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();
rangeDetector.addListener(featureListener);
touchDetector.addListener(featureListener);
lightDetector.addListener(featureListener);
/* 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);
behaviors = new Behavior[3];
behaviors[0] = new WanderBehavior(pilot, sonar, touch, light, compass, WHEEL_RADIUS, TRACK_WIDTH);
behaviors[1] = new SensorCalibrationBehavior(sonar, touch, light, compass, WHEEL_RADIUS, TRACK_WIDTH);
behaviors[2] = new AvoidObstaclesBehavior(pilot, sonar, touch, light, compass, WHEEL_RADIUS, TRACK_WIDTH);
/* Start the program. */
arbitrator = new Arbitrator(behaviors, true);

73
src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java Normal file
View File

@@ -0,0 +1,73 @@
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.navigation.ArcRotateMoveController;
import ve.ucv.ciens.cicore.icaro.ryabi.sensors.SensorEvent;
import ve.ucv.ciens.cicore.icaro.ryabi.sensors.SensorEventsQueue;
public class AvoidObstaclesBehavior extends BaseBehavior {
private SensorEventsQueue queue;
public AvoidObstaclesBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) {
super(pilot, sonar, touch, light, compass, wheelRadius, trackWidth);
this.queue = SensorEventsQueue.getInstance();
}
@Override
public boolean takeControl() {
return queue.hasNextTouchSensorEvent();
}
@Override
public void action() {
SensorEvent event = queue.getNextTouchSensorEvent();
pilot.stop();
pilot.travel(-100);
rotate90();
pilot.travel(250);
rotateM90();
pilot.stop();
if(!queue.hasNextTouchSensorEvent())
event.detector.enableDetection(true);
}
@Override
public void suppress() { }
private void rotate90() {
float cMeasure;
compass.resetCartesianZero();
pilot.setRotateSpeed(25);
pilot.rotate(Integer.MIN_VALUE - 1, true);
cMeasure = 360.0f;
try { Thread.sleep(200); } catch (InterruptedException e) { }
while(cMeasure > 270.0f) {
cMeasure = compass.getDegreesCartesian();
}
pilot.stop();
}
private void rotateM90() {
float cMeasure;
compass.resetCartesianZero();
pilot.setRotateSpeed(25);
pilot.rotate(-3000, true);
cMeasure = 0.0f;
try { Thread.sleep(200); } catch (InterruptedException e) { }
while(cMeasure < 90.0f) {
cMeasure = compass.getDegreesCartesian();
}
pilot.stop();
}
}

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

@@ -4,19 +4,26 @@ import lejos.nxt.LightSensor;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassHTSensor;
import lejos.robotics.navigation.ArcRotateMoveController;
import lejos.robotics.subsumption.Behavior;
public abstract class BaseBehavior implements Behavior {
protected UltrasonicSensor sonar;
protected TouchSensor touch;
protected CompassHTSensor compass;
protected LightSensor light;
protected ArcRotateMoveController pilot;
protected UltrasonicSensor sonar;
protected TouchSensor touch;
protected CompassHTSensor compass;
protected LightSensor light;
protected float wheelRadius;
protected float trackWidth;
public BaseBehavior(UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass) {
public BaseBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) {
this.pilot = pilot;
this.sonar = sonar;
this.touch = touch;
this.compass = compass;
this.light = light;
this.wheelRadius = wheelRadius;
this.trackWidth = trackWidth;
}
@Override

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

@@ -1,60 +0,0 @@
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);
}
}

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

@@ -10,14 +10,10 @@ 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);
super(null, sonar, touch, light, compass, wheelRadius, trackWidth);
sensorsCalibrated = false;
this.wheelRadius = wheelRadius;
this.trackWidth = trackWidth;
}
@Override

40
src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/WanderBehavior.java Normal file
View File

@@ -0,0 +1,40 @@
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 ve.ucv.ciens.cicore.icaro.ryabi.utils.RobotStateSingleton;
@SuppressWarnings("deprecation")
public class WanderBehavior extends BaseBehavior {
private RobotStateSingleton state;
public WanderBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) {
super(pilot, sonar, touch, light, compass, wheelRadius, trackWidth);
state = RobotStateSingleton.getInstance();
pilot = new CompassPilot(compass, 56f, 155f, Motor.A, Motor.C);
}
@Override
public boolean takeControl() {
return state.getState() == RobotStateSingleton.States.WANDER;
}
@Override
public void action() {
if(!pilot.isMoving())
pilot.forward();
}
@Override
public void suppress() {
if(pilot.isMoving())
pilot.stop();
}
}

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

@@ -1,57 +0,0 @@
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();
}
}

21
src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectionListener.java Normal file
View File

@@ -0,0 +1,21 @@
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
import lejos.nxt.Sound;
import lejos.robotics.objectdetection.Feature;
import lejos.robotics.objectdetection.FeatureDetector;
import lejos.robotics.objectdetection.FeatureListener;
public class FeatureDetectionListener implements FeatureListener {
private SensorEventsQueue eventsQueue;
public FeatureDetectionListener() {
this.eventsQueue = SensorEventsQueue.getInstance();
}
@Override
public void featureDetected(Feature feature, FeatureDetector detector) {
detector.enableDetection(false);
eventsQueue.addEvent(feature, detector);
Sound.beep();
}
}

2
src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/LightFeatureDetector.java → src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/LightFeatureDetector.java
View File

@@ -1,4 +1,4 @@
package ve.ucv.ciens.cicore.icaro.ryabi.detectors;
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
import lejos.geom.Point;
import lejos.nxt.LightSensor;

14
src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEvent.java Normal file
View File

@@ -0,0 +1,14 @@
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
import lejos.robotics.objectdetection.Feature;
import lejos.robotics.objectdetection.FeatureDetector;
public class SensorEvent {
public Feature feature;
public FeatureDetector detector;
public SensorEvent(Feature feature, FeatureDetector detector) {
this.feature = feature;
this.detector = detector;
}
}

73
src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEventsQueue.java Normal file
View File

@@ -0,0 +1,73 @@
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
import java.util.Queue;
import lejos.robotics.objectdetection.Feature;
import lejos.robotics.objectdetection.FeatureDetector;
import lejos.robotics.objectdetection.RangeFeatureDetector;
import lejos.robotics.objectdetection.TouchFeatureDetector;
public final class SensorEventsQueue {
private Queue<SensorEvent> touchEventsQueue;
private Queue<SensorEvent> lightEventsQueue;
private Queue<SensorEvent> sonarEventsQueue;
private SensorEventsQueue() {
touchEventsQueue = new Queue<SensorEvent>();
lightEventsQueue = new Queue<SensorEvent>();
sonarEventsQueue = new Queue<SensorEvent>();
}
private static class SingletonHolder {
private static final SensorEventsQueue INSTANCE = new SensorEventsQueue();
}
public static SensorEventsQueue getInstance() {
return SingletonHolder.INSTANCE;
}
public synchronized void addEvent(Feature feature, FeatureDetector detector) {
SensorEvent event = new SensorEvent(feature, detector);
if(detector instanceof TouchFeatureDetector) {
touchEventsQueue.addElement(event);
} else if(detector instanceof LightFeatureDetector) {
lightEventsQueue.addElement(event);
} else if(detector instanceof RangeFeatureDetector) {
sonarEventsQueue.addElement(event);
}
}
public synchronized SensorEvent getNextTouchSensorEvent() {
if(!touchEventsQueue.empty())
return (SensorEvent) touchEventsQueue.pop();
else
return null;
}
public synchronized SensorEvent getNextLightSensorEvent() {
if(!lightEventsQueue.empty())
return (SensorEvent) lightEventsQueue.pop();
else
return null;
}
public synchronized SensorEvent getNextSonarSensorEvent() {
if(!sonarEventsQueue.empty())
return (SensorEvent) sonarEventsQueue.pop();
else
return null;
}
public synchronized boolean hasNextTouchSensorEvent() {
return !touchEventsQueue.empty();
}
public synchronized boolean hasNextLightSensorEvent() {
return !lightEventsQueue.empty();
}
public synchronized boolean hasNextSonarSensorEvent() {
return !sonarEventsQueue.empty();
}
}

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

@@ -1,29 +0,0 @@
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) { }
}
}
}

29
src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java Normal file
View File

@@ -0,0 +1,29 @@
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
public final class RobotStateSingleton {
private States state;
private RobotStateSingleton() {
state = States.WANDER;
}
private static class SingletonHolder {
private static final RobotStateSingleton INSTANCE = new RobotStateSingleton();
}
public static RobotStateSingleton getInstance() {
return SingletonHolder.INSTANCE;
}
public void setState(States state) {
this.state = state;
}
public States getState() {
return this.state;
}
public enum States {
WANDER, SEARCH_BALL, BALL_FOUND, VICTORY;
}
}