Fixed up to ball search.

src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java

@@ -78,7 +78,7 @@ public class RyABI {
 		pilot = new CompassPilot(compass, WHEEL_DIAMETER, TRACK_WIDTH, Motor.A, Motor.C);
 
 		/* Create the feature detectors. */
-		rangeDetector = new RangeFeatureDetector(sonar, 20, 200);
+		rangeDetector = new RangeFeatureDetector(sonar, 30, 200);
 		touchDetector = new TouchFeatureDetector(touch);
 		lightDetector = new LightFeatureDetector(light, invertLightDetector);
 

src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java

@@ -96,66 +96,64 @@ public class SearchBallBehavior extends BaseBehavior {
 
 	@Override
 	public void action() {
-		while(!supress) {
-			if(queue.hasNextRangeSensorEvent()) {
-				/* If the pedestal has been found then stop the robot and set the state to BALL_FOUND. */
-				if(pilot.isMoving())
-					pilot.stop();
-				ballFound = true;
-				state.setState(States.BALL_FOUND);
+		if(queue.hasNextRangeSensorEvent()) {
+			/* If the pedestal has been found then stop the robot and set the state to BALL_FOUND. */
+			if(pilot.isMoving())
+				pilot.stop();
+			ballFound = true;
+			state.setState(States.BALL_FOUND);
 
-				/* Discard unneeded range features. */
-				while(queue.hasNextRangeSensorEvent())
-					queue.getNextRangeSensorEvent();
+			/* Discard unneeded range features. */
+			while(queue.hasNextRangeSensorEvent())
+				queue.getNextRangeSensorEvent();
+
+		} else {
+			if(turnLeft) {
+				/* Search to the left of the robot. */
+				Rotations.rotateM90(compass, pilot);
+				pilot.travel(100);
+
+				if(queue.hasNextTouchSensorEvent()) {
+					/* If an obstacle is found while searching then start searching
+					 * to the opposite side. */
+					pilot.travel(-200);
+
+					turnLeft = false;
+
+					/* Discard unneeded touch events. */
+					while(queue.hasNextTouchSensorEvent())
+						queue.getNextTouchSensorEvent();
+
+					detectorHandler.enableTouchDetector();
+				}
+
+				Rotations.rotate90(compass, pilot);
 
 			} else {
-				if(turnLeft) {
-					/* Search to the left of the robot. */
-					Rotations.rotateM90(compass, pilot);
-					pilot.travel(100);
+				/* Search to the right of the robot. */
+				Rotations.rotate90(compass, pilot);
+				pilot.travel(100);
 
-					if(queue.hasNextTouchSensorEvent()) {
-						/* If an obstacle is found while searching then start searching
-						 * to the opposite side. */
-						pilot.travel(-200);
+				if(queue.hasNextTouchSensorEvent()) {
+					/* If an obstacle is found while searching then give up and go back
+					 * to the start line. */
+					pilot.travel(-200);
 
-						turnLeft = false;
+					state.setState(States.WANDER);
+					ballFound = true;
 
-						/* Discard unneeded touch events. */
-						while(queue.hasNextTouchSensorEvent())
-							queue.getNextTouchSensorEvent();
+					/* Discard unneeded touch events. */
+					while(queue.hasNextTouchSensorEvent())
+						queue.getNextTouchSensorEvent();
 
-						detectorHandler.enableTouchDetector();
-					}
-
-					Rotations.rotate90(compass, pilot);
-
-				} else {
-					/* Search to the right of the robot. */
-					Rotations.rotate90(compass, pilot);
-					pilot.travel(100);
-
-					if(queue.hasNextTouchSensorEvent()) {
-						/* If an obstacle is found while searching then give up and go back
-						 * to the start line. */
-						pilot.travel(-200);
-
-						state.setState(States.WANDER);
-						ballFound = true;
-
-						/* Discard unneeded touch events. */
-						while(queue.hasNextTouchSensorEvent())
-							queue.getNextTouchSensorEvent();
-
-						detectorHandler.enableTouchDetector();
-					}
-
-					Rotations.rotateM90(compass, pilot);
-
-					/* If the ball was found or the robot gave up then turn around towards the start line. */
-					if(ballFound)
-						Rotations.rotate180(compass, pilot);
+					detectorHandler.enableTouchDetector();
 				}
+
+				Rotations.rotateM90(compass, pilot);
+
+				/* If the ball was found or the robot gave up then turn around towards the start line. */
+				if(ballFound)
+					Rotations.rotate180(compass, pilot);
 			}
 		}
 	}

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

@@ -54,7 +54,7 @@ public class SensorCalibrationBehavior extends BaseBehavior {
 		/* Calibrate the compass by turning slowly by 720 degrees. */
 		System.out.println("Calib. compass");
 		DifferentialPilot p = new DifferentialPilot(wheelDiameter, trackWidth, Motor.A, Motor.C);
-		p.setRotateSpeed(25);
+		p.setRotateSpeed(20);
 		compass.startCalibration();
 		p.rotate(720, false);
 		compass.stopCalibration();

src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java

@@ -36,8 +36,9 @@ public abstract class Rotations {
 		diff = 90.0f - cMeasure;
 		do {
 			pilot.rotate(diff);
-			cMeasure = compass.getDegreesCartesian();
+			cMeasure = 360.0f - compass.getDegreesCartesian();
 			diff = 90.0f - cMeasure;
+			System.out.println(cMeasure  + " " + diff);
 		} while(Math.abs(diff) > 1.0f);
 
 		pilot.stop();
@@ -60,6 +61,7 @@ public abstract class Rotations {
 			pilot.rotate(diff);
 			cMeasure = compass.getDegreesCartesian();
 			diff = -90 + cMeasure;
+			System.out.println(cMeasure  + " " + diff);
 		} while(Math.abs(diff) > 1.0f);
 
 		pilot.stop();