WallyHackenslacker/RyABI
1
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

Search ball almost ready. Victory ready.

Browse Source
This commit is contained in:
Miguel Angel Astor Romero
2016-06-23 15:49:30 -04:00
parent 2f829de04b
commit 1a11653b93
6 changed files with 120 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java
View File

@@ -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);

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

@@ -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);
}

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

@@ -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);
}
}

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

@@ -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);

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

@@ -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";
}
}
}

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

@@ -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();