diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java index 1c179f0..a796999 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java @@ -1,6 +1,7 @@ package ve.ucv.ciens.cicore.icaro.ryabi; import lejos.nxt.Button; +import lejos.nxt.LCD; import lejos.nxt.LightSensor; import lejos.nxt.Motor; import lejos.nxt.SensorPort; @@ -43,19 +44,22 @@ public class RyABI { private static FeatureDetectorsHandler detectorHandler; public static void main(String[] args) { + boolean invertLightDetector; /* Create the sensors. */ sonar = new UltrasonicSensor(SensorPort.S1); touch = new TouchSensor(SensorPort.S2); compass = new CompassHTSensor(SensorPort.S3); light = new LightSensor(SensorPort.S4); + invertLightDetector = invertLightDetector(); + /* Create the pilot. */ pilot = new CompassPilot(compass, WHEEL_DIAMETER, TRACK_WIDTH, Motor.A, Motor.C); /* Create the feature detectors. */ rangeDetector = new RangeFeatureDetector(sonar, 20, 200); touchDetector = new TouchFeatureDetector(touch); - lightDetector = new LightFeatureDetector(light); + lightDetector = new LightFeatureDetector(light, invertLightDetector); detectorHandler = FeatureDetectorsHandler.getInstance(); detectorHandler.setRangeDetector(rangeDetector); @@ -71,17 +75,60 @@ public class RyABI { /* Register escape button for forced exit. */ Button.ESCAPE.addButtonListener(new QuitButtonListener()); + /* Start the sensor calibration. */ + behaviors = new Behavior[1]; + behaviors[0] = new SensorCalibrationBehavior(sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); + arbitrator = new Arbitrator(behaviors, true); + arbitrator.start(); + + /* Wait for the user to press the center button. */ + System.out.println("Sensors ready."); + System.out.println("Press ENTER"); + System.out.println("to start."); + Button.ENTER.waitForPress(); + /* Create the behaviors. */ - behaviors = new Behavior[6]; + behaviors = new Behavior[5]; behaviors[0] = new WanderBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); behaviors[1] = new VictoryBehavior(); behaviors[2] = new SearchBoxBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); behaviors[3] = new CatchBallBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); behaviors[4] = new AvoidObstaclesBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); - behaviors[5] = new SensorCalibrationBehavior(sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); /* Start the program. */ arbitrator = new Arbitrator(behaviors, true); arbitrator.start(); } + + private static boolean invertLightDetector() { + int btnID; + boolean invertSensor = false, done = false; + + while(!done) { + LCD.clear(); + System.out.println("Invert l. sens:"); + if(invertSensor) + System.out.println("<< YES >>"); + else + System.out.println("<< NO >>"); + System.out.println("Press ENTER"); + System.out.println("to set."); + + btnID = Button.waitForAnyPress(); + + switch(btnID) { + case Button.ID_LEFT: + case Button.ID_RIGHT: + invertSensor = !invertSensor; + break; + case Button.ID_ENTER: + done = true; + break; + default: + System.exit(1); + } + } + + return invertSensor; + } } diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java index dfbba4c..3863f9d 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java @@ -23,13 +23,14 @@ public class AvoidObstaclesBehavior extends BaseBehavior { @Override public boolean takeControl() { - return state.getState() == States.WANDER && queue.hasNextTouchSensorEvent(); + return queue.hasNextTouchSensorEvent() && state.getState() == States.WANDER; } @Override public void action() { SensorEvent event = queue.getNextTouchSensorEvent(); + event.detector.enableDetection(false); pilot.stop(); pilot.travel(-100); Rotations.rotate90(compass, pilot); @@ -37,8 +38,10 @@ public class AvoidObstaclesBehavior extends BaseBehavior { Rotations.rotateM90(compass, pilot); pilot.stop(); - if(!queue.hasNextTouchSensorEvent()) - event.detector.enableDetection(true); + while(queue.hasNextTouchSensorEvent()) + event = queue.getNextTouchSensorEvent(); + + event.detector.enableDetection(true); } @Override diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBoxBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBoxBehavior.java index c0f3ea7..bd5ee06 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBoxBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBoxBehavior.java @@ -54,6 +54,7 @@ public class SearchBoxBehavior extends BaseBehavior { @Override public void action() { if(queue.hasNextRangeSensorEvent()) { + if(pilot.isMoving()) pilot.stop(); ballFound = true; diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/LightFeatureDetector.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/LightFeatureDetector.java index 7cf5eca..5f981f5 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/LightFeatureDetector.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/LightFeatureDetector.java @@ -46,7 +46,7 @@ public class LightFeatureDetector extends FeatureDetectorAdapter { this.threshold = threshold; this.invert = invert; - // Calculate angle a distance of bumper from center: + // Calculate angle and distance of light sensor 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); @@ -56,9 +56,16 @@ public class LightFeatureDetector extends FeatureDetectorAdapter { @Override public Feature scan() { RangeFeature rf = null; - if(lightSensor.getLightValue() <= threshold) { - RangeReading rr = new RangeReading(angle, range); - rf = new RangeFeature(rr); + if(invert) { + if(lightSensor.getLightValue() >= threshold) { + RangeReading rr = new RangeReading(angle, range); + rf = new RangeFeature(rr); + } + } else { + if(lightSensor.getLightValue() < threshold) { + RangeReading rr = new RangeReading(angle, range); + rf = new RangeFeature(rr); + } } return rf; } @@ -70,7 +77,7 @@ public class LightFeatureDetector extends FeatureDetectorAdapter { if(invert) { while(lightSensor.getLightValue() >= threshold); } else { - while(lightSensor.getLightValue() <= threshold); + while(lightSensor.getLightValue() < threshold); } } }