Fixed a bug in obstacle avoidance.

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

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,7 +38,9 @@ public class AvoidObstaclesBehavior extends BaseBehavior {
 		Rotations.rotateM90(compass, pilot);
 		pilot.stop();
 
-		if(!queue.hasNextTouchSensorEvent())
+		while(queue.hasNextTouchSensorEvent())
+			event = queue.getNextTouchSensorEvent();
+
 		event.detector.enableDetection(true);
 	}
 

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;

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,10 +56,17 @@ public class LightFeatureDetector extends FeatureDetectorAdapter {
 	@Override
 	public Feature scan() {
 		RangeFeature rf = null;
-		if(lightSensor.getLightValue() <= threshold) {
+		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);
 		}
 	}
 }