From 5289603a82c736c0b0e25c7d4abc1e962150cd5a Mon Sep 17 00:00:00 2001
From: Miguel Angel Astor Romero <miguel.astor@ciens.ucv.ve>
Date: Thu, 23 Jun 2016 13:00:59 -0400
Subject: [PATCH] Changed the rotation functions.

---
 .../ryabi/behaviors/SearchBallBehavior.java   |  4 +-
 .../cicore/icaro/ryabi/utils/Rotations.java   | 57 ++++++++-----------
 2 files changed, 26 insertions(+), 35 deletions(-)

diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java
index 13af4ac..3e69623 100644
--- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java
+++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java
@@ -117,7 +117,7 @@ public class SearchBallBehavior extends BaseBehavior {
 					if(queue.hasNextTouchSensorEvent()) {
 						/* If an obstacle is found while searching then start searching
 						 * to the opposite side. */
-						pilot.travel(-100);
+						pilot.travel(-200);
 
 						turnLeft = false;
 
@@ -138,7 +138,7 @@ public class SearchBallBehavior extends BaseBehavior {
 					if(queue.hasNextTouchSensorEvent()) {
 						/* If an obstacle is found while searching then give up and go back
 						 * to the start line. */
-						pilot.travel(-100);
+						pilot.travel(-200);
 
 						state.setState(States.WANDER);
 						ballFound = true;
diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java
index c3d0f37..39b8db2 100644
--- a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java
+++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java
@@ -28,21 +28,18 @@ public abstract class Rotations {
 	 * @param pilot The {@link ArcRotateMoveController} implementation that controls the robot.
 	 */
 	public static void rotate90(CompassHTSensor compass, ArcRotateMoveController pilot) {
-		float cMeasure;
+		float cMeasure = 0.0f, diff;
 
-		/* First reset the compass. */
+		pilot.stop();
 		compass.resetCartesianZero();
-
-		/* Start rotating slowly. */
 		pilot.setRotateSpeed(25);
-		pilot.rotate(Integer.MIN_VALUE - 1, true);
-
-		/* Keep rotating until the robot has turned 90 degrees. */
-		cMeasure = 360.0f;
-		try { Thread.sleep(200); } catch (InterruptedException e) { }
-		while(cMeasure > 270.0f) {
+		diff = 90.0f - cMeasure;
+		do {
+			pilot.rotate(diff);
 			cMeasure = compass.getDegreesCartesian();
-		}
+			diff = 90.0f - cMeasure;
+		} while(Math.abs(diff) > 1.0f);
+
 		pilot.stop();
 	}
 
@@ -53,21 +50,18 @@ public abstract class Rotations {
 	 * @param pilot The {@link ArcRotateMoveController} implementation that controls the robot.
 	 */
 	public static void rotateM90(CompassHTSensor compass, ArcRotateMoveController pilot) {
-		float cMeasure;
+		float cMeasure = 0.0f, diff;
 
-		/* First reset the compass. */
+		pilot.stop();
 		compass.resetCartesianZero();
-
-		/* Start rotating slowly. */
 		pilot.setRotateSpeed(25);
-		pilot.rotate(-3000, true);
-
-		/* Keep rotating until the robot has turned 90 degrees. */
-		cMeasure = 0.0f;
-		try { Thread.sleep(200); } catch (InterruptedException e) { }
-		while(cMeasure < 90.0f) {
+		diff = -90;
+		do {
+			pilot.rotate(diff);
 			cMeasure = compass.getDegreesCartesian();
-		}
+			diff = -90 + cMeasure;
+		} while(Math.abs(diff) > 1.0f);
+
 		pilot.stop();
 	}
 
@@ -78,21 +72,18 @@ public abstract class Rotations {
 	 * @param pilot The {@link ArcRotateMoveController} implementation that controls the robot.
 	 */
 	public static void rotate180(CompassHTSensor compass, ArcRotateMoveController pilot) {
-		float cMeasure;
+		float cMeasure = 0.0f, diff = 0.0f;
 
-		/* First reset the compass. */
+		pilot.stop();
 		compass.resetCartesianZero();
-
-		/* Start rotating slowly. */
 		pilot.setRotateSpeed(25);
-		pilot.rotate(Integer.MIN_VALUE - 1, true);
-
-		/* Keep rotating until the robot has turned 90 degrees. */
-		cMeasure = 360.0f;
-		try { Thread.sleep(200); } catch (InterruptedException e) { }
-		while(cMeasure > 180.0f) {
+		diff = 180.0f - cMeasure;
+		do {
+			pilot.rotate(diff);
 			cMeasure = compass.getDegreesCartesian();
-		}
+			diff = 180.0f - cMeasure;
+		} while(Math.abs(diff) > 1.0f);
+
 		pilot.stop();
 	}
 }