Search ball almost ready. Victory ready.

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

@@ -111,7 +111,7 @@ public class RyABI {
 		/* Create the behaviors. */
 		behaviors = new Behavior[5];
 		behaviors[0] = new WanderBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH);
-		behaviors[1] = new VictoryBehavior();
+		behaviors[1] = new VictoryBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH);
 		behaviors[2] = new SearchBallBehavior(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);

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

@@ -56,9 +56,11 @@ public class CatchBallBehavior extends BaseBehavior {
 		pilot.travel(65);
 		Motor.B.backward();
 		try { Thread.sleep(2000); } catch(InterruptedException e) { };
-
+		Motor.B.stop(true);
 		/* Turn towards the start line and start moving. */
-		Rotations.rotate180(compass, pilot);
+		Rotations.rotate90(compass, pilot);
+		Rotations.rotate90(compass, pilot);
+		pilot.travel(200);
 		state.setState(States.WANDER);
 	}
 

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

@@ -40,7 +40,6 @@ public class SearchBallBehavior extends BaseBehavior {
 	private FeatureDetectorsHandler detectorHandler;
 	private RobotStateSingleton     state;
 	private boolean                 turnLeft;
-	private boolean                 supress;
 
 	/**
 	 * Creates a new {@link SearchBallBehavior}.
@@ -59,7 +58,6 @@ public class SearchBallBehavior extends BaseBehavior {
 		this.detectorHandler = FeatureDetectorsHandler.getInstance();
 		this.state = RobotStateSingleton.getInstance();
 		this.turnLeft = true;
-		this.supress = false;
 	}
 
 	@Override
@@ -71,7 +69,6 @@ public class SearchBallBehavior extends BaseBehavior {
 		/* If the current state is SEARCH_BALL then set the detectors and take control. */
 		if(state.getState() == States.SEARCH_BALL) {
 			setDetectors();
-			this.supress = false;
 			return true;
 		}
 
@@ -85,8 +82,6 @@ public class SearchBallBehavior extends BaseBehavior {
 			while(queue.hasNextLightSensorEvent())
 				queue.getNextLightSensorEvent();
 
-			this.supress = false;
-
 			return true;
 
 		}
@@ -96,74 +91,35 @@ public class SearchBallBehavior extends BaseBehavior {
 
 	@Override
 	public void action() {
-		if(queue.hasNextRangeSensorEvent()) {
+		boolean obstacleFound = false;
-			/* 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. */
+		/* Discard unneeded touch events. */
-			while(queue.hasNextRangeSensorEvent())
+		while(queue.hasNextTouchSensorEvent())
-				queue.getNextRangeSensorEvent();
+			queue.getNextTouchSensorEvent();
 
+		if(turnLeft) {
+			moveLeft();
 		} else {
-			if(turnLeft) {
+			obstacleFound = moveRight();
-				/* Search to the left of the robot. */
+		}
-				Rotations.rotateM90(compass, pilot);
-				pilot.travel(100);
 
-				if(queue.hasNextTouchSensorEvent()) {
+		pilot.stop();
-					/* If an obstacle is found while searching then start searching
-					 * to the opposite side. */
-					pilot.travel(-200);
 
-					turnLeft = false;
+		if(!obstacleFound) {
-
+			ballFound = searchBall();
-					/* Discard unneeded touch events. */
+			if(ballFound)
-					while(queue.hasNextTouchSensorEvent())
+				state.setState(States.BALL_FOUND);
-						queue.getNextTouchSensorEvent();
+		} else {
-
+			ballFound = true;
-					detectorHandler.enableTouchDetector();
+			Rotations.rotate90(compass, pilot);
-				}
+			Rotations.rotate90(compass, pilot);
-
+			pilot.travel(250);
-				Rotations.rotate90(compass, pilot);
+			state.setState(States.WANDER);
-
-			} 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);
-			}
 		}
 	}
 
 	@Override
-	public void suppress() {
+	public void suppress() { }
-		if(pilot.isMoving())
-			pilot.stop();
-		this.supress = true;
-	}
 
 	/**
 	 * Enables the touch and range feature detectors and disables the light feature detector.
@@ -171,6 +127,73 @@ public class SearchBallBehavior extends BaseBehavior {
 	private void setDetectors() {
 		detectorHandler.enableTouchDetector();
 		detectorHandler.disableLightDetector();
+		detectorHandler.disableRangeDetector();
+	}
+
+	private void moveLeft() {
+		/* Search to the left of the robot. */
+		Rotations.rotateM90(compass, pilot);
+		pilot.travel(100);
+
+		turnLeft = !obstacle();
+
+		Rotations.rotate90(compass, pilot);
+	}
+
+	private boolean moveRight() {
+		boolean obstacleFound = false;
+
+		/* Search to the right of the robot. */
+		Rotations.rotate90(compass, pilot);
+		pilot.travel(100);
+
+		obstacleFound = obstacle();
+
+		Rotations.rotateM90(compass, pilot);
+
+		return obstacleFound;
+	}
+
+	private boolean searchBall() {
+		boolean found = false;
+
 		detectorHandler.enableRangeDetector();
+		activeWait(1000);
+		if(queue.hasNextRangeSensorEvent()) {
+			/* If the pedestal has been found then stop the robot and set the state to BALL_FOUND. */
+			state.setState(States.BALL_FOUND);
+
+			/* Discard unneeded range features. */
+			while(queue.hasNextRangeSensorEvent())
+				queue.getNextRangeSensorEvent();
+		}
+		detectorHandler.disableRangeDetector();
+
+		return found;
+	}
+
+	private boolean obstacle() {
+		boolean obstacleFound = false;
+
+		if(queue.hasNextTouchSensorEvent()) {
+			pilot.travel(-200);
+
+			/* Discard unneeded touch events. */
+			while(queue.hasNextTouchSensorEvent())
+				queue.getNextTouchSensorEvent();
+
+			detectorHandler.enableTouchDetector();
+			obstacleFound = true;
+		}
+
+		return obstacleFound;
+	}
+
+	private void activeWait(long milliseconds) {
+		long then = System.currentTimeMillis(), now;
+		System.out.println("Waiting.");
+		do {
+			now = System.currentTimeMillis();
+		} while(now - then < 1000);
 	}
 }

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

@@ -15,7 +15,12 @@ package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
 
 import java.awt.Point;
 
+import lejos.nxt.LightSensor;
 import lejos.nxt.Sound;
+import lejos.nxt.TouchSensor;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.navigation.ArcRotateMoveController;
 import lejos.robotics.subsumption.Behavior;
 import ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectorsHandler;
 import ve.ucv.ciens.cicore.icaro.ryabi.sensors.SensorEventsQueue;
@@ -28,7 +33,7 @@ import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
  * @author Miguel Angel Astor Romero.
  */
 @SuppressWarnings("unused")
-public class VictoryBehavior implements Behavior {
+public class VictoryBehavior extends BaseBehavior {
 	private static final int C       = 262;
 	private static final int D       = 287;
 	private static final int E       = 320;
@@ -49,7 +54,9 @@ public class VictoryBehavior implements Behavior {
 	/**
 	 * Creates a new {@link VictoryBehavior}.
 	 */
-	public VictoryBehavior() {
+	public VictoryBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
+		super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth);
+
 		state = RobotStateSingleton.getInstance();
 		this.queue = SensorEventsQueue.getInstance();
 		this.detectorHandler = FeatureDetectorsHandler.getInstance();
@@ -101,6 +108,7 @@ public class VictoryBehavior implements Behavior {
 
 	@Override
 	public void action() {
+		pilot.stop();
 		/* Play some music! */
 		playMusic(score);
 

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

@@ -44,6 +44,7 @@ public final class RobotStateSingleton {
 	 * @param state the state to set.
 	 */
 	public synchronized void setState(States state) {
+		System.out.println(getStateName(state));
 		this.state = state;
 	}
 
@@ -55,4 +56,21 @@ public final class RobotStateSingleton {
 	public States getState() {
 		return this.state;
 	}
+	
+	private String getStateName(States state) {
+		switch (state) {
+		case BALL_FOUND:
+			return "BALL FOUND";
+		case DONE:
+			return "DONE";
+		case SEARCH_BALL:
+			return "SEARCH BALL";
+		case VICTORY:
+			return "VICTORY";
+		case WANDER:
+			return "WANDER";
+		default:
+			return "ERROR";
+		}
+	}
 }

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

@@ -38,7 +38,6 @@ public abstract class Rotations {
 			pilot.rotate(diff);
 			cMeasure = 360.0f - compass.getDegreesCartesian();
 			diff = 90.0f - cMeasure;
-			System.out.println(cMeasure  + " " + diff);
 		} while(Math.abs(diff) > 1.0f);
 
 		pilot.stop();
@@ -61,29 +60,6 @@ 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();
-	}
-
-	/**
-	 * Rotates the robot 180 degrees clockwise.
-	 * 
-	 * @param compass A {@link CompassHTSensor} to use to make good turns.
-	 * @param pilot The {@link ArcRotateMoveController} implementation that controls the robot.
-	 */
-	public static void rotate180(CompassHTSensor compass, ArcRotateMoveController pilot) {
-		float cMeasure = 0.0f, diff = 0.0f;
-
-		pilot.stop();
-		compass.resetCartesianZero();
-		pilot.setRotateSpeed(25);
-		diff = 180.0f - cMeasure;
-		do {
-			pilot.rotate(diff);
-			cMeasure = compass.getDegreesCartesian();
-			diff = 180.0f - cMeasure;
 		} while(Math.abs(diff) > 1.0f);
 
 		pilot.stop();