diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java index 078f1d6..b6dedda 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java @@ -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); diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java new file mode 100644 index 0000000..fba4943 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java @@ -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(); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/BaseBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/BaseBehavior.java index bb9b962..14950c1 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/BaseBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/BaseBehavior.java @@ -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 diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/GetToBorderBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/GetToBorderBehavior.java deleted file mode 100644 index 990181b..0000000 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/GetToBorderBehavior.java +++ /dev/null @@ -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); - } -} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java index bb9f763..601632c 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java @@ -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 diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/WanderBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/WanderBehavior.java new file mode 100644 index 0000000..1fa2203 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/WanderBehavior.java @@ -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(); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/FeatureDetectionListener.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/FeatureDetectionListener.java deleted file mode 100644 index 222cf1d..0000000 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/FeatureDetectionListener.java +++ /dev/null @@ -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(); - } -} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectionListener.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectionListener.java new file mode 100644 index 0000000..78484b4 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectionListener.java @@ -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(); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/LightFeatureDetector.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/LightFeatureDetector.java similarity index 97% rename from src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/LightFeatureDetector.java rename to src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/LightFeatureDetector.java index 77fdad6..7cf5eca 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/detectors/LightFeatureDetector.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/LightFeatureDetector.java @@ -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; diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEvent.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEvent.java new file mode 100644 index 0000000..394e536 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEvent.java @@ -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; + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEventsQueue.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEventsQueue.java new file mode 100644 index 0000000..6589543 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEventsQueue.java @@ -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 touchEventsQueue; + private Queue lightEventsQueue; + private Queue sonarEventsQueue; + + private SensorEventsQueue() { + touchEventsQueue = new Queue(); + lightEventsQueue = new Queue(); + sonarEventsQueue = new Queue(); + } + + 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(); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/threads/CompassThread.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/threads/CompassThread.java deleted file mode 100644 index 11bc8d4..0000000 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/threads/CompassThread.java +++ /dev/null @@ -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) { } - } - } -} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java new file mode 100644 index 0000000..e418801 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java @@ -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; + } +}