diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java index b96c2c8..79aeddb 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java +++ b/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); diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java index 6662d8f..57997d7 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java +++ b/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); } 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 defb76d..34a2d0b 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java +++ b/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()) { - /* 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); + boolean obstacleFound = false; - /* Discard unneeded range features. */ - while(queue.hasNextRangeSensorEvent()) - queue.getNextRangeSensorEvent(); + /* Discard unneeded touch events. */ + while(queue.hasNextTouchSensorEvent()) + queue.getNextTouchSensorEvent(); + if(turnLeft) { + moveLeft(); } else { - if(turnLeft) { - /* Search to the left of the robot. */ - Rotations.rotateM90(compass, pilot); - pilot.travel(100); + obstacleFound = moveRight(); + } - if(queue.hasNextTouchSensorEvent()) { - /* If an obstacle is found while searching then start searching - * to the opposite side. */ - pilot.travel(-200); + pilot.stop(); - turnLeft = false; - - /* 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); - } + if(!obstacleFound) { + ballFound = searchBall(); + if(ballFound) + state.setState(States.BALL_FOUND); + } else { + ballFound = true; + Rotations.rotate90(compass, pilot); + Rotations.rotate90(compass, pilot); + pilot.travel(250); + state.setState(States.WANDER); } } @Override - public void suppress() { - if(pilot.isMoving()) - pilot.stop(); - this.supress = true; - } + public void suppress() { } /** * 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); } } diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/VictoryBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/VictoryBehavior.java index 70f5d8e..f973984 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/VictoryBehavior.java +++ b/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); diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java index 069fe28..503c1d4 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java +++ b/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"; + } + } } 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 f73ab0c..aad4537 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java +++ b/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();