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 9582ba7..13af4ac 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java @@ -40,6 +40,7 @@ public class SearchBallBehavior extends BaseBehavior { private FeatureDetectorsHandler detectorHandler; private RobotStateSingleton state; private boolean turnLeft; + private boolean supress; /** * Creates a new {@link SearchBallBehavior}. @@ -58,6 +59,7 @@ public class SearchBallBehavior extends BaseBehavior { this.detectorHandler = FeatureDetectorsHandler.getInstance(); this.state = RobotStateSingleton.getInstance(); this.turnLeft = true; + this.supress = false; } @Override @@ -69,6 +71,7 @@ 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; } @@ -82,6 +85,8 @@ public class SearchBallBehavior extends BaseBehavior { while(queue.hasNextLightSensorEvent()) queue.getNextLightSensorEvent(); + this.supress = false; + return true; } @@ -91,64 +96,66 @@ public class SearchBallBehavior extends BaseBehavior { @Override public void action() { - 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); + 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); - /* 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(-100); - - turnLeft = false; - - /* Discard unneeded touch events. */ - while(queue.hasNextTouchSensorEvent()) - queue.getNextTouchSensorEvent(); - - detectorHandler.enableTouchDetector(); - } - - Rotations.rotate90(compass, pilot); + /* Discard unneeded range features. */ + while(queue.hasNextRangeSensorEvent()) + queue.getNextRangeSensorEvent(); } else { - /* Search to the right of the robot. */ - Rotations.rotate90(compass, pilot); - pilot.travel(100); + 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 give up and go back - * to the start line. */ - pilot.travel(-100); + if(queue.hasNextTouchSensorEvent()) { + /* If an obstacle is found while searching then start searching + * to the opposite side. */ + pilot.travel(-100); - state.setState(States.WANDER); - ballFound = true; + turnLeft = false; - /* Discard unneeded touch events. */ - while(queue.hasNextTouchSensorEvent()) - queue.getNextTouchSensorEvent(); + /* Discard unneeded touch events. */ + while(queue.hasNextTouchSensorEvent()) + queue.getNextTouchSensorEvent(); - detectorHandler.enableTouchDetector(); + 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(-100); + + 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); } - - 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); } } } @@ -157,6 +164,7 @@ public class SearchBallBehavior extends BaseBehavior { public void suppress() { if(pilot.isMoving()) pilot.stop(); + this.supress = true; } /**