diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java index 9ca0333..1c179f0 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java @@ -14,11 +14,14 @@ import lejos.robotics.objectdetection.TouchFeatureDetector; import lejos.robotics.subsumption.Arbitrator; import lejos.robotics.subsumption.Behavior; import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.AvoidObstaclesBehavior; -import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.WanderBehavior; -import ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectionListener; -import ve.ucv.ciens.cicore.icaro.ryabi.sensors.LightFeatureDetector; +import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.CatchBallBehavior; +import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.SearchBoxBehavior; import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.SensorCalibrationBehavior; import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.VictoryBehavior; +import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.WanderBehavior; +import ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectorsHandler; +import ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectionListener; +import ve.ucv.ciens.cicore.icaro.ryabi.sensors.LightFeatureDetector; import ve.ucv.ciens.cicore.icaro.ryabi.utils.QuitButtonListener; @SuppressWarnings("deprecation") @@ -37,8 +40,9 @@ public class RyABI { private static TouchFeatureDetector touchDetector; private static LightFeatureDetector lightDetector; private static FeatureDetectionListener featureListener; + private static FeatureDetectorsHandler detectorHandler; - public static void main(String[] args) { + public static void main(String[] args) { /* Create the sensors. */ sonar = new UltrasonicSensor(SensorPort.S1); touch = new TouchSensor(SensorPort.S2); @@ -48,14 +52,16 @@ public class RyABI { /* Create the pilot. */ pilot = new CompassPilot(compass, WHEEL_DIAMETER, TRACK_WIDTH, Motor.A, Motor.C); - /* Create the feature detectors and their listener. */ + /* Create the feature detectors. */ rangeDetector = new RangeFeatureDetector(sonar, 20, 200); touchDetector = new TouchFeatureDetector(touch); lightDetector = new LightFeatureDetector(light); - rangeDetector.enableDetection(true); - touchDetector.enableDetection(true); - lightDetector.enableDetection(true); + detectorHandler = FeatureDetectorsHandler.getInstance(); + detectorHandler.setRangeDetector(rangeDetector); + detectorHandler.setTouchDetector(touchDetector); + detectorHandler.setLightDetector(lightDetector); + detectorHandler.disableAllDetectors(); featureListener = new FeatureDetectionListener(); rangeDetector.addListener(featureListener); @@ -66,11 +72,13 @@ public class RyABI { Button.ESCAPE.addButtonListener(new QuitButtonListener()); /* Create the behaviors. */ - behaviors = new Behavior[4]; + behaviors = new Behavior[6]; behaviors[0] = new WanderBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); - behaviors[1] = new SensorCalibrationBehavior(sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); - behaviors[2] = new AvoidObstaclesBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); - behaviors[3] = new VictoryBehavior(pilot, sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); + behaviors[1] = new VictoryBehavior(); + behaviors[2] = new SearchBoxBehavior(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); + behaviors[5] = new SensorCalibrationBehavior(sonar, touch, light, compass, WHEEL_DIAMETER, TRACK_WIDTH); /* Start the program. */ arbitrator = new Arbitrator(behaviors, true); diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java index fba4943..dfbba4c 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/AvoidObstaclesBehavior.java @@ -7,18 +7,23 @@ import lejos.nxt.addon.CompassHTSensor; import lejos.robotics.navigation.ArcRotateMoveController; import ve.ucv.ciens.cicore.icaro.ryabi.sensors.SensorEvent; import ve.ucv.ciens.cicore.icaro.ryabi.sensors.SensorEventsQueue; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.RobotStateSingleton; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.Rotations; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.States; public class AvoidObstaclesBehavior extends BaseBehavior { - private SensorEventsQueue queue; + private RobotStateSingleton state; + private SensorEventsQueue queue; public AvoidObstaclesBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) { super(pilot, sonar, touch, light, compass, wheelRadius, trackWidth); + this.state = RobotStateSingleton.getInstance(); this.queue = SensorEventsQueue.getInstance(); } @Override public boolean takeControl() { - return queue.hasNextTouchSensorEvent(); + return state.getState() == States.WANDER && queue.hasNextTouchSensorEvent(); } @Override @@ -27,9 +32,9 @@ public class AvoidObstaclesBehavior extends BaseBehavior { pilot.stop(); pilot.travel(-100); - rotate90(); + Rotations.rotate90(compass, pilot); pilot.travel(250); - rotateM90(); + Rotations.rotateM90(compass, pilot); pilot.stop(); if(!queue.hasNextTouchSensorEvent()) @@ -38,36 +43,4 @@ public class AvoidObstaclesBehavior extends BaseBehavior { @Override public void suppress() { } - - private void rotate90() { - float cMeasure; - - compass.resetCartesianZero(); - - pilot.setRotateSpeed(25); - pilot.rotate(Integer.MIN_VALUE - 1, true); - - cMeasure = 360.0f; - try { Thread.sleep(200); } catch (InterruptedException e) { } - while(cMeasure > 270.0f) { - cMeasure = compass.getDegreesCartesian(); - } - pilot.stop(); - } - - private void rotateM90() { - float cMeasure; - - compass.resetCartesianZero(); - - pilot.setRotateSpeed(25); - pilot.rotate(-3000, true); - - cMeasure = 0.0f; - try { Thread.sleep(200); } catch (InterruptedException e) { } - while(cMeasure < 90.0f) { - cMeasure = compass.getDegreesCartesian(); - } - pilot.stop(); - } } diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java new file mode 100644 index 0000000..03c0edc --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/CatchBallBehavior.java @@ -0,0 +1,35 @@ +package ve.ucv.ciens.cicore.icaro.ryabi.behaviors; + +import lejos.nxt.LightSensor; +import lejos.nxt.TouchSensor; +import lejos.nxt.UltrasonicSensor; +import lejos.nxt.addon.CompassHTSensor; +import lejos.robotics.navigation.ArcRotateMoveController; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.RobotStateSingleton; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.Rotations; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.States; + +public class CatchBallBehavior extends BaseBehavior { + RobotStateSingleton state; + + public CatchBallBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) { + super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth); + this.state = RobotStateSingleton.getInstance(); + } + + @Override + public boolean takeControl() { + return state.getState() == States.BALL_FOUND; + } + + @Override + public void action() { + // TODO Close claw. + + Rotations.rotate180(compass, pilot); + state.setState(States.WANDER); + } + + @Override + public void suppress() { } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBoxBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBoxBehavior.java new file mode 100644 index 0000000..c0f3ea7 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBoxBehavior.java @@ -0,0 +1,119 @@ +package ve.ucv.ciens.cicore.icaro.ryabi.behaviors; + +import lejos.nxt.LightSensor; +import lejos.nxt.TouchSensor; +import lejos.nxt.UltrasonicSensor; +import lejos.nxt.addon.CompassHTSensor; +import lejos.robotics.navigation.ArcRotateMoveController; +import ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectorsHandler; +import ve.ucv.ciens.cicore.icaro.ryabi.sensors.SensorEventsQueue; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.RobotStateSingleton; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.Rotations; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.States; + +public class SearchBoxBehavior extends BaseBehavior { + private SensorEventsQueue queue; + private boolean ballFound; + private FeatureDetectorsHandler detectorHandler; + private RobotStateSingleton state; + private boolean turnLeft; + + public SearchBoxBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) { + super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth); + this.queue = SensorEventsQueue.getInstance(); + this.ballFound = false; + this.detectorHandler = FeatureDetectorsHandler.getInstance(); + this.state = RobotStateSingleton.getInstance(); + this.turnLeft = true; + } + + @Override + public boolean takeControl() { + if(ballFound) + return false; + + if(state.getState() == States.SEARCH_BALL) { + setDetectors(); + return true; + } + + if(queue.hasNextLightSensorEvent()) { + state.setState(States.SEARCH_BALL); + setDetectors(); + + while(queue.hasNextLightSensorEvent()) + queue.getNextLightSensorEvent(); + + return true; + + } + + return false; + } + + @Override + public void action() { + if(queue.hasNextRangeSensorEvent()) { + if(pilot.isMoving()) + pilot.stop(); + ballFound = true; + state.setState(States.BALL_FOUND); + + while(queue.hasNextRangeSensorEvent()) + queue.getNextRangeSensorEvent(); + + } else { + if(turnLeft) { + Rotations.rotateM90(compass, pilot); + pilot.travel(50); + + if(queue.hasNextTouchSensorEvent()) { + pilot.travel(-100); + + turnLeft = false; + + while(queue.hasNextTouchSensorEvent()) + queue.getNextTouchSensorEvent(); + + detectorHandler.enableTouchDetector(); + } + + Rotations.rotate90(compass, pilot); + + } else { + Rotations.rotate90(compass, pilot); + pilot.travel(50); + + if(queue.hasNextTouchSensorEvent()) { + pilot.travel(-100); + + state.setState(States.WANDER); + ballFound = true; + + while(queue.hasNextTouchSensorEvent()) + queue.getNextTouchSensorEvent(); + + detectorHandler.enableTouchDetector(); + } + + Rotations.rotateM90(compass, pilot); + + if(ballFound) { + Rotations.rotate180(compass, pilot); + } + } + } + } + + @Override + public void suppress() { + if(pilot.isMoving()) + pilot.stop(); + } + + private void setDetectors() { + detectorHandler.enableTouchDetector(); + detectorHandler.disableLightDetector(); + detectorHandler.enableRangeDetector(); + } +} 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 36da29b..00d71ce 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/VictoryBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/VictoryBehavior.java @@ -1,44 +1,81 @@ package ve.ucv.ciens.cicore.icaro.ryabi.behaviors; -import java.io.File; - -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 ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectorsHandler; +import ve.ucv.ciens.cicore.icaro.ryabi.sensors.SensorEventsQueue; import ve.ucv.ciens.cicore.icaro.ryabi.utils.RobotStateSingleton; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.States; +@SuppressWarnings("unused") public class VictoryBehavior extends BaseBehavior { - private RobotStateSingleton state; - private File musicSample; + private static final int C = 262; + private static final int D = 287; + private static final int E = 320; + private static final int F = 349; + private static final int G = 392; + private static final int A = 440; + private static final int B = 494; - public VictoryBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, - CompassHTSensor compass, float wheelDiameter, float trackWidth) { - super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth); + private RobotStateSingleton state; + private SensorEventsQueue queue; + private FeatureDetectorsHandler detectorHandler; + public VictoryBehavior() { + super(null, null, null, null, null, 0.0f, 0.0f); state = RobotStateSingleton.getInstance(); - musicSample = new File("victory.wav"); - if(!musicSample.exists()) - musicSample = null; + this.queue = SensorEventsQueue.getInstance(); + this.detectorHandler = FeatureDetectorsHandler.getInstance(); } @Override public boolean takeControl() { - return state.getState() == RobotStateSingleton.States.VICTORY; + if(state.getState() == States.VICTORY) { + detectorHandler.disableAllDetectors(); + return true; + } + + if(queue.hasNextLightSensorEvent()) { + state.setState(States.VICTORY); + detectorHandler.disableAllDetectors(); + + while(queue.hasNextLightSensorEvent()) + queue.getNextLightSensorEvent(); + + return true; + + } + + return false; } @Override public void action() { - if(musicSample != null) { - int milis = Sound.playSample(musicSample); - try { Thread.sleep(milis); } catch(InterruptedException ie) { } - } - state.setState(RobotStateSingleton.States.DONE); + playToneDuration(B, 250); + playToneDuration(A, 250); + playToneDuration(G, 500); + playToneDuration(B, 250); + playToneDuration(A, 250); + playToneDuration(G, 500); + playToneDuration(G, 125); + playToneDuration(G, 125); + playToneDuration(G, 125); + playToneDuration(G, 125); + playToneDuration(A, 125); + playToneDuration(A, 125); + playToneDuration(A, 125); + playToneDuration(A, 125); + playToneDuration(B, 250); + playToneDuration(A, 250); + playToneDuration(G, 500); + state.setState(States.DONE); } @Override public void suppress() { } + + private void playToneDuration(int tone, int milis) { + Sound.playTone(tone, milis); + try { Thread.sleep(milis); } catch(InterruptedException ie) {} + } } diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/WanderBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/WanderBehavior.java index d23d224..2c5edc6 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/WanderBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/WanderBehavior.java @@ -1,29 +1,34 @@ package ve.ucv.ciens.cicore.icaro.ryabi.behaviors; import lejos.nxt.LightSensor; -import lejos.nxt.Motor; import lejos.nxt.TouchSensor; import lejos.nxt.UltrasonicSensor; import lejos.nxt.addon.CompassHTSensor; import lejos.robotics.navigation.ArcRotateMoveController; -import lejos.robotics.navigation.CompassPilot; +import ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectorsHandler; import ve.ucv.ciens.cicore.icaro.ryabi.utils.RobotStateSingleton; +import ve.ucv.ciens.cicore.icaro.ryabi.utils.States; -@SuppressWarnings("deprecation") public class WanderBehavior extends BaseBehavior { - private RobotStateSingleton state; + private RobotStateSingleton state; + private FeatureDetectorsHandler detectorHandler; public WanderBehavior(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(); - - pilot = new CompassPilot(compass, 56f, 155f, Motor.A, Motor.C); + detectorHandler = FeatureDetectorsHandler.getInstance(); } @Override public boolean takeControl() { - return state.getState() == RobotStateSingleton.States.WANDER; + if(state.getState() == States.WANDER) { + detectorHandler.enableTouchDetector(); + detectorHandler.enableLightDetector(); + detectorHandler.disableRangeDetector(); + return true; + + } else + return false; } @Override diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectorsHandler.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectorsHandler.java new file mode 100644 index 0000000..c5d7f03 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/FeatureDetectorsHandler.java @@ -0,0 +1,84 @@ +package ve.ucv.ciens.cicore.icaro.ryabi.sensors; + +import lejos.robotics.objectdetection.RangeFeatureDetector; +import lejos.robotics.objectdetection.TouchFeatureDetector; + +public final class FeatureDetectorsHandler { + private RangeFeatureDetector rangeDetector; + private TouchFeatureDetector touchDetector; + private LightFeatureDetector lightDetector; + + private FeatureDetectorsHandler() { + rangeDetector = null; + touchDetector = null; + lightDetector = null; + } + + private static class SingletonHolder { + private static final FeatureDetectorsHandler INSTANCE = new FeatureDetectorsHandler(); + } + + public static FeatureDetectorsHandler getInstance() { + return SingletonHolder.INSTANCE; + } + + public synchronized void setRangeDetector(RangeFeatureDetector rangeDetector) { + this.rangeDetector = rangeDetector; + } + + public synchronized void setTouchDetector(TouchFeatureDetector touchDetector) { + this.touchDetector = touchDetector; + } + + public synchronized void setLightDetector(LightFeatureDetector lightDetector) { + this.lightDetector = lightDetector; + } + + public synchronized void enableRangeDetector() { + if(rangeDetector != null) + rangeDetector.enableDetection(true); + } + + public synchronized void disableRangeDetector() { + if(rangeDetector != null) + rangeDetector.enableDetection(false); + } + + public synchronized void enableTouchDetector() { + if(touchDetector != null) + touchDetector.enableDetection(true); + } + + public synchronized void disableTouchDetector() { + if(touchDetector != null) + touchDetector.enableDetection(false); + } + + public synchronized void enableLightDetector() { + if(lightDetector != null) + lightDetector.enableDetection(true); + } + + public synchronized void disableLightDetector() { + if(lightDetector != null) + lightDetector.enableDetection(false); + } + + public synchronized void enableAllDetectors() { + if(rangeDetector != null) + rangeDetector.enableDetection(true); + if(touchDetector != null) + touchDetector.enableDetection(true); + if(lightDetector != null) + lightDetector.enableDetection(true); + } + + public synchronized void disableAllDetectors() { + if(rangeDetector != null) + rangeDetector.enableDetection(false); + if(touchDetector != null) + touchDetector.enableDetection(false); + if(lightDetector != null) + lightDetector.enableDetection(false); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEventsQueue.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEventsQueue.java index 6589543..8b826bd 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEventsQueue.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/sensors/SensorEventsQueue.java @@ -10,12 +10,12 @@ import lejos.robotics.objectdetection.TouchFeatureDetector; public final class SensorEventsQueue { private Queue touchEventsQueue; private Queue lightEventsQueue; - private Queue sonarEventsQueue; + private Queue rangeEventsQueue; private SensorEventsQueue() { touchEventsQueue = new Queue(); lightEventsQueue = new Queue(); - sonarEventsQueue = new Queue(); + rangeEventsQueue = new Queue(); } private static class SingletonHolder { @@ -34,7 +34,7 @@ public final class SensorEventsQueue { } else if(detector instanceof LightFeatureDetector) { lightEventsQueue.addElement(event); } else if(detector instanceof RangeFeatureDetector) { - sonarEventsQueue.addElement(event); + rangeEventsQueue.addElement(event); } } @@ -52,9 +52,9 @@ public final class SensorEventsQueue { return null; } - public synchronized SensorEvent getNextSonarSensorEvent() { - if(!sonarEventsQueue.empty()) - return (SensorEvent) sonarEventsQueue.pop(); + public synchronized SensorEvent getNextRangeSensorEvent() { + if(!rangeEventsQueue.empty()) + return (SensorEvent) rangeEventsQueue.pop(); else return null; } @@ -67,7 +67,7 @@ public final class SensorEventsQueue { return !lightEventsQueue.empty(); } - public synchronized boolean hasNextSonarSensorEvent() { - return !sonarEventsQueue.empty(); + public synchronized boolean hasNextRangeSensorEvent() { + return !rangeEventsQueue.empty(); } } 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 3337721..68e20bd 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/RobotStateSingleton.java @@ -22,8 +22,4 @@ public final class RobotStateSingleton { public States getState() { return this.state; } - - public enum States { - WANDER, SEARCH_BALL, BALL_FOUND, VICTORY, DONE; - } } diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java new file mode 100644 index 0000000..ed6bcb8 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java @@ -0,0 +1,54 @@ +package ve.ucv.ciens.cicore.icaro.ryabi.utils; + +import lejos.nxt.addon.CompassHTSensor; +import lejos.robotics.navigation.ArcRotateMoveController; + +public abstract class Rotations { + public static void rotate90(CompassHTSensor compass, ArcRotateMoveController pilot) { + float cMeasure; + + compass.resetCartesianZero(); + + pilot.setRotateSpeed(25); + pilot.rotate(Integer.MIN_VALUE - 1, true); + + cMeasure = 360.0f; + try { Thread.sleep(200); } catch (InterruptedException e) { } + while(cMeasure > 270.0f) { + cMeasure = compass.getDegreesCartesian(); + } + pilot.stop(); + } + + public static void rotateM90(CompassHTSensor compass, ArcRotateMoveController pilot) { + float cMeasure; + + compass.resetCartesianZero(); + + pilot.setRotateSpeed(25); + pilot.rotate(-3000, true); + + cMeasure = 0.0f; + try { Thread.sleep(200); } catch (InterruptedException e) { } + while(cMeasure < 90.0f) { + cMeasure = compass.getDegreesCartesian(); + } + pilot.stop(); + } + + public static void rotate180(CompassHTSensor compass, ArcRotateMoveController pilot) { + float cMeasure; + + compass.resetCartesianZero(); + + pilot.setRotateSpeed(25); + pilot.rotate(Integer.MIN_VALUE - 1, true); + + cMeasure = 360.0f; + try { Thread.sleep(200); } catch (InterruptedException e) { } + while(cMeasure > 180.0f) { + cMeasure = compass.getDegreesCartesian(); + } + pilot.stop(); + } +} diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/States.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/States.java new file mode 100644 index 0000000..af2a960 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/States.java @@ -0,0 +1,5 @@ +package ve.ucv.ciens.cicore.icaro.ryabi.utils; + +public enum States { + WANDER, SEARCH_BALL, BALL_FOUND, VICTORY, DONE; +}