Added comments and javadoc everywhere.
This commit is contained in:
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi;
|
package ve.ucv.ciens.cicore.icaro.ryabi;
|
||||||
|
|
||||||
import lejos.nxt.Button;
|
import lejos.nxt.Button;
|
||||||
@@ -25,6 +38,11 @@ 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.sensors.LightFeatureDetector;
|
||||||
import ve.ucv.ciens.cicore.icaro.ryabi.utils.QuitButtonListener;
|
import ve.ucv.ciens.cicore.icaro.ryabi.utils.QuitButtonListener;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class starts the execution of the program.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
@SuppressWarnings("deprecation")
|
@SuppressWarnings("deprecation")
|
||||||
public class RyABI {
|
public class RyABI {
|
||||||
private static final float WHEEL_DIAMETER = 56.0f;
|
private static final float WHEEL_DIAMETER = 56.0f;
|
||||||
@@ -43,6 +61,9 @@ public class RyABI {
|
|||||||
private static FeatureDetectionListener featureListener;
|
private static FeatureDetectionListener featureListener;
|
||||||
private static FeatureDetectorsHandler detectorHandler;
|
private static FeatureDetectorsHandler detectorHandler;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Main method of the program.
|
||||||
|
*/
|
||||||
public static void main(String[] args) {
|
public static void main(String[] args) {
|
||||||
boolean invertLightDetector;
|
boolean invertLightDetector;
|
||||||
/* Create the sensors. */
|
/* Create the sensors. */
|
||||||
@@ -100,11 +121,17 @@ public class RyABI {
|
|||||||
arbitrator.start();
|
arbitrator.start();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Asks the user to select the light feature detection mode.
|
||||||
|
*
|
||||||
|
* @return True if the user wants to invert the light detector (report when the robot passes from a dark area to a light area).
|
||||||
|
*/
|
||||||
private static boolean invertLightDetector() {
|
private static boolean invertLightDetector() {
|
||||||
int btnID;
|
int btnID;
|
||||||
boolean invertSensor = false, done = false;
|
boolean invertSensor = false, done = false;
|
||||||
|
|
||||||
while(!done) {
|
while(!done) {
|
||||||
|
/* Show the prompt on the screen. */
|
||||||
LCD.clear();
|
LCD.clear();
|
||||||
System.out.println("Invert l. sens:");
|
System.out.println("Invert l. sens:");
|
||||||
if(invertSensor)
|
if(invertSensor)
|
||||||
@@ -114,17 +141,21 @@ public class RyABI {
|
|||||||
System.out.println("Press ENTER");
|
System.out.println("Press ENTER");
|
||||||
System.out.println("to set.");
|
System.out.println("to set.");
|
||||||
|
|
||||||
|
/* Wait for any button press. */
|
||||||
btnID = Button.waitForAnyPress();
|
btnID = Button.waitForAnyPress();
|
||||||
|
|
||||||
switch(btnID) {
|
switch(btnID) {
|
||||||
case Button.ID_LEFT:
|
case Button.ID_LEFT:
|
||||||
case Button.ID_RIGHT:
|
case Button.ID_RIGHT:
|
||||||
|
/* If the user pressed any direction button then toggle the sensor. */
|
||||||
invertSensor = !invertSensor;
|
invertSensor = !invertSensor;
|
||||||
break;
|
break;
|
||||||
case Button.ID_ENTER:
|
case Button.ID_ENTER:
|
||||||
|
/* If the user pressed enter then end the loop. */
|
||||||
done = true;
|
done = true;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
|
/* If the user pressed escape then quit. */
|
||||||
System.exit(1);
|
System.exit(1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
||||||
|
|
||||||
import lejos.nxt.LightSensor;
|
import lejos.nxt.LightSensor;
|
||||||
@@ -11,10 +24,19 @@ 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.Rotations;
|
||||||
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class implements a {@link BaseBehavior} that attempts to evade obstacles detected by the {@link TouchSensor} by moving
|
||||||
|
* a bit to the right.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class AvoidObstaclesBehavior extends BaseBehavior {
|
public class AvoidObstaclesBehavior extends BaseBehavior {
|
||||||
private RobotStateSingleton state;
|
private RobotStateSingleton state;
|
||||||
private SensorEventsQueue queue;
|
private SensorEventsQueue queue;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link AvoidObstaclesBehavior}.
|
||||||
|
*/
|
||||||
public AvoidObstaclesBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) {
|
public AvoidObstaclesBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) {
|
||||||
super(pilot, sonar, touch, light, compass, wheelRadius, trackWidth);
|
super(pilot, sonar, touch, light, compass, wheelRadius, trackWidth);
|
||||||
this.state = RobotStateSingleton.getInstance();
|
this.state = RobotStateSingleton.getInstance();
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
||||||
|
|
||||||
import lejos.nxt.LightSensor;
|
import lejos.nxt.LightSensor;
|
||||||
@@ -7,6 +20,11 @@ import lejos.nxt.addon.CompassHTSensor;
|
|||||||
import lejos.robotics.navigation.ArcRotateMoveController;
|
import lejos.robotics.navigation.ArcRotateMoveController;
|
||||||
import lejos.robotics.subsumption.Behavior;
|
import lejos.robotics.subsumption.Behavior;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Base class for {@link Behavior} implementations that need to use multiple sensors and a pilot.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public abstract class BaseBehavior implements Behavior {
|
public abstract class BaseBehavior implements Behavior {
|
||||||
protected ArcRotateMoveController pilot;
|
protected ArcRotateMoveController pilot;
|
||||||
protected UltrasonicSensor sonar;
|
protected UltrasonicSensor sonar;
|
||||||
@@ -16,6 +34,17 @@ public abstract class BaseBehavior implements Behavior {
|
|||||||
protected float wheelDiameter;
|
protected float wheelDiameter;
|
||||||
protected float trackWidth;
|
protected float trackWidth;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link BaseBehavior}. Has to be called by subclasses.
|
||||||
|
*
|
||||||
|
* @param pilot A differential pilot that implements {@link ArcRotateMoveController}
|
||||||
|
* @param sonar A range finder sensor.
|
||||||
|
* @param touch A touch sensor.
|
||||||
|
* @param light A monochromatic light sensor.
|
||||||
|
* @param compass A HiTechnic compass sensor.
|
||||||
|
* @param wheelDiameter The diameter of the active wheels. Assumes both wheels are the same size.
|
||||||
|
* @param trackWidth The distance between the center of both active wheels.
|
||||||
|
*/
|
||||||
public BaseBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
|
public BaseBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
|
||||||
this.pilot = pilot;
|
this.pilot = pilot;
|
||||||
this.sonar = sonar;
|
this.sonar = sonar;
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
||||||
|
|
||||||
import lejos.nxt.LightSensor;
|
import lejos.nxt.LightSensor;
|
||||||
@@ -10,9 +23,24 @@ 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.Rotations;
|
||||||
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class implements a {@link BaseBehavior} that just closes the B {@link Motor} in order to catch the ball after it has been found.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class CatchBallBehavior extends BaseBehavior {
|
public class CatchBallBehavior extends BaseBehavior {
|
||||||
RobotStateSingleton state;
|
RobotStateSingleton state;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link CatchBallBehavior}.
|
||||||
|
* @param pilot
|
||||||
|
* @param sonar
|
||||||
|
* @param touch
|
||||||
|
* @param light
|
||||||
|
* @param compass
|
||||||
|
* @param wheelDiameter
|
||||||
|
* @param trackWidth
|
||||||
|
*/
|
||||||
public CatchBallBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
|
public CatchBallBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
|
||||||
super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth);
|
super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth);
|
||||||
this.state = RobotStateSingleton.getInstance();
|
this.state = RobotStateSingleton.getInstance();
|
||||||
@@ -28,6 +56,7 @@ public class CatchBallBehavior extends BaseBehavior {
|
|||||||
Motor.B.forward();
|
Motor.B.forward();
|
||||||
try { Thread.sleep(2000); } catch(InterruptedException e) { };
|
try { Thread.sleep(2000); } catch(InterruptedException e) { };
|
||||||
|
|
||||||
|
/* Turn towards the start line and start moving. */
|
||||||
Rotations.rotate180(compass, pilot);
|
Rotations.rotate180(compass, pilot);
|
||||||
state.setState(States.WANDER);
|
state.setState(States.WANDER);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
||||||
|
|
||||||
import lejos.nxt.LightSensor;
|
import lejos.nxt.LightSensor;
|
||||||
@@ -11,6 +24,16 @@ 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.Rotations;
|
||||||
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class implements a {@link BaseBehavior} that searches slowly to the left and right of the robot for the pedestal where the ball
|
||||||
|
* should be in. It first searches to the left of the robot until it hits an obstacle. Then it will search to the right of the robot
|
||||||
|
* until it hits another obstacle.
|
||||||
|
*
|
||||||
|
* If the ball is not found after searching on both sides then the robot will make a 180 degrees turn and will start moving back
|
||||||
|
* to the start line.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class SearchBallBehavior extends BaseBehavior {
|
public class SearchBallBehavior extends BaseBehavior {
|
||||||
private SensorEventsQueue queue;
|
private SensorEventsQueue queue;
|
||||||
private boolean ballFound;
|
private boolean ballFound;
|
||||||
@@ -18,6 +41,16 @@ public class SearchBallBehavior extends BaseBehavior {
|
|||||||
private RobotStateSingleton state;
|
private RobotStateSingleton state;
|
||||||
private boolean turnLeft;
|
private boolean turnLeft;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link SearchBallBehavior}.
|
||||||
|
* @param pilot
|
||||||
|
* @param sonar
|
||||||
|
* @param touch
|
||||||
|
* @param light
|
||||||
|
* @param compass
|
||||||
|
* @param wheelDiameter
|
||||||
|
* @param trackWidth
|
||||||
|
*/
|
||||||
public SearchBallBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
|
public SearchBallBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
|
||||||
super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth);
|
super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth);
|
||||||
this.queue = SensorEventsQueue.getInstance();
|
this.queue = SensorEventsQueue.getInstance();
|
||||||
@@ -29,18 +62,23 @@ public class SearchBallBehavior extends BaseBehavior {
|
|||||||
|
|
||||||
@Override
|
@Override
|
||||||
public boolean takeControl() {
|
public boolean takeControl() {
|
||||||
|
/* If the ball has already been found then this behavior should not take control again. */
|
||||||
if(ballFound)
|
if(ballFound)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
|
/* If the current state is SEARCH_BALL then set the detectors and take control. */
|
||||||
if(state.getState() == States.SEARCH_BALL) {
|
if(state.getState() == States.SEARCH_BALL) {
|
||||||
setDetectors();
|
setDetectors();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* If the state is not SEARCH_BALL but there is at least one light feature detected
|
||||||
|
* indicating that the finish line has been reached then set the detectors and take control. */
|
||||||
if(queue.hasNextLightSensorEvent()) {
|
if(queue.hasNextLightSensorEvent()) {
|
||||||
state.setState(States.SEARCH_BALL);
|
state.setState(States.SEARCH_BALL);
|
||||||
setDetectors();
|
setDetectors();
|
||||||
|
|
||||||
|
/* Discard unneeded detected light features. */
|
||||||
while(queue.hasNextLightSensorEvent())
|
while(queue.hasNextLightSensorEvent())
|
||||||
queue.getNextLightSensorEvent();
|
queue.getNextLightSensorEvent();
|
||||||
|
|
||||||
@@ -54,25 +92,30 @@ public class SearchBallBehavior extends BaseBehavior {
|
|||||||
@Override
|
@Override
|
||||||
public void action() {
|
public void action() {
|
||||||
if(queue.hasNextRangeSensorEvent()) {
|
if(queue.hasNextRangeSensorEvent()) {
|
||||||
|
/* If the pedestal has been found then stop the robot and set the state to BALL_FOUND. */
|
||||||
if(pilot.isMoving())
|
if(pilot.isMoving())
|
||||||
pilot.stop();
|
pilot.stop();
|
||||||
ballFound = true;
|
ballFound = true;
|
||||||
state.setState(States.BALL_FOUND);
|
state.setState(States.BALL_FOUND);
|
||||||
|
|
||||||
|
/* Discard unneeded range features. */
|
||||||
while(queue.hasNextRangeSensorEvent())
|
while(queue.hasNextRangeSensorEvent())
|
||||||
queue.getNextRangeSensorEvent();
|
queue.getNextRangeSensorEvent();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if(turnLeft) {
|
if(turnLeft) {
|
||||||
|
/* Search to the left of the robot. */
|
||||||
Rotations.rotateM90(compass, pilot);
|
Rotations.rotateM90(compass, pilot);
|
||||||
pilot.travel(50);
|
pilot.travel(50);
|
||||||
|
|
||||||
if(queue.hasNextTouchSensorEvent()) {
|
if(queue.hasNextTouchSensorEvent()) {
|
||||||
|
/* If an obstacle is found while searching then start searching
|
||||||
|
* to the opposite side. */
|
||||||
pilot.travel(-100);
|
pilot.travel(-100);
|
||||||
|
|
||||||
turnLeft = false;
|
turnLeft = false;
|
||||||
|
|
||||||
|
/* Discard unneeded touch events. */
|
||||||
while(queue.hasNextTouchSensorEvent())
|
while(queue.hasNextTouchSensorEvent())
|
||||||
queue.getNextTouchSensorEvent();
|
queue.getNextTouchSensorEvent();
|
||||||
|
|
||||||
@@ -82,15 +125,19 @@ public class SearchBallBehavior extends BaseBehavior {
|
|||||||
Rotations.rotate90(compass, pilot);
|
Rotations.rotate90(compass, pilot);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
/* Search to the right of the robot. */
|
||||||
Rotations.rotate90(compass, pilot);
|
Rotations.rotate90(compass, pilot);
|
||||||
pilot.travel(50);
|
pilot.travel(50);
|
||||||
|
|
||||||
if(queue.hasNextTouchSensorEvent()) {
|
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(-100);
|
||||||
|
|
||||||
state.setState(States.WANDER);
|
state.setState(States.WANDER);
|
||||||
ballFound = true;
|
ballFound = true;
|
||||||
|
|
||||||
|
/* Discard unneeded touch events. */
|
||||||
while(queue.hasNextTouchSensorEvent())
|
while(queue.hasNextTouchSensorEvent())
|
||||||
queue.getNextTouchSensorEvent();
|
queue.getNextTouchSensorEvent();
|
||||||
|
|
||||||
@@ -99,12 +146,12 @@ public class SearchBallBehavior extends BaseBehavior {
|
|||||||
|
|
||||||
Rotations.rotateM90(compass, pilot);
|
Rotations.rotateM90(compass, pilot);
|
||||||
|
|
||||||
if(ballFound) {
|
/* If the ball was found or the robot gave up then turn around towards the start line. */
|
||||||
|
if(ballFound)
|
||||||
Rotations.rotate180(compass, pilot);
|
Rotations.rotate180(compass, pilot);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void suppress() {
|
public void suppress() {
|
||||||
@@ -112,6 +159,9 @@ public class SearchBallBehavior extends BaseBehavior {
|
|||||||
pilot.stop();
|
pilot.stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Enables the touch and range feature detectors and disables the light feature detector.
|
||||||
|
*/
|
||||||
private void setDetectors() {
|
private void setDetectors() {
|
||||||
detectorHandler.enableTouchDetector();
|
detectorHandler.enableTouchDetector();
|
||||||
detectorHandler.disableLightDetector();
|
detectorHandler.disableLightDetector();
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
||||||
|
|
||||||
import lejos.nxt.Button;
|
import lejos.nxt.Button;
|
||||||
@@ -8,9 +21,24 @@ import lejos.nxt.UltrasonicSensor;
|
|||||||
import lejos.nxt.addon.CompassHTSensor;
|
import lejos.nxt.addon.CompassHTSensor;
|
||||||
import lejos.robotics.navigation.DifferentialPilot;
|
import lejos.robotics.navigation.DifferentialPilot;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class implements a {@link BaseBehavior} that performs automatic calibration of the {@link CompassHTSensor} and {@link LightSensor}.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class SensorCalibrationBehavior extends BaseBehavior {
|
public class SensorCalibrationBehavior extends BaseBehavior {
|
||||||
private boolean sensorsCalibrated;
|
private boolean sensorsCalibrated;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link SensorCalibrationBehavior}
|
||||||
|
*
|
||||||
|
* @param sonar
|
||||||
|
* @param touch
|
||||||
|
* @param light
|
||||||
|
* @param compass
|
||||||
|
* @param wheelRadius
|
||||||
|
* @param trackWidth
|
||||||
|
*/
|
||||||
public SensorCalibrationBehavior(UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) {
|
public SensorCalibrationBehavior(UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelRadius, float trackWidth) {
|
||||||
super(null, sonar, touch, light, compass, wheelRadius, trackWidth);
|
super(null, sonar, touch, light, compass, wheelRadius, trackWidth);
|
||||||
sensorsCalibrated = false;
|
sensorsCalibrated = false;
|
||||||
@@ -23,6 +51,7 @@ public class SensorCalibrationBehavior extends BaseBehavior {
|
|||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void action() {
|
public void action() {
|
||||||
|
/* Calibrate the compass by turning slowly by 720 degrees. */
|
||||||
System.out.println("Calib. compass");
|
System.out.println("Calib. compass");
|
||||||
DifferentialPilot p = new DifferentialPilot(wheelDiameter, trackWidth, Motor.A, Motor.C);
|
DifferentialPilot p = new DifferentialPilot(wheelDiameter, trackWidth, Motor.A, Motor.C);
|
||||||
p.setRotateSpeed(25);
|
p.setRotateSpeed(25);
|
||||||
@@ -30,6 +59,7 @@ public class SensorCalibrationBehavior extends BaseBehavior {
|
|||||||
p.rotate(720, false);
|
p.rotate(720, false);
|
||||||
compass.stopCalibration();
|
compass.stopCalibration();
|
||||||
|
|
||||||
|
/* Ask the user for input in calibrating the light sensor. */
|
||||||
System.out.println("Calib. light s.");
|
System.out.println("Calib. light s.");
|
||||||
light.setFloodlight(true);
|
light.setFloodlight(true);
|
||||||
System.out.println("Point at dark\nand press ENTER");
|
System.out.println("Point at dark\nand press ENTER");
|
||||||
|
|||||||
@@ -1,13 +1,34 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
||||||
|
|
||||||
|
import java.awt.Point;
|
||||||
|
|
||||||
import lejos.nxt.Sound;
|
import lejos.nxt.Sound;
|
||||||
|
import lejos.robotics.subsumption.Behavior;
|
||||||
import ve.ucv.ciens.cicore.icaro.ryabi.sensors.FeatureDetectorsHandler;
|
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.sensors.SensorEventsQueue;
|
||||||
import ve.ucv.ciens.cicore.icaro.ryabi.utils.RobotStateSingleton;
|
import ve.ucv.ciens.cicore.icaro.ryabi.utils.RobotStateSingleton;
|
||||||
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class implements a {@link Behavior} that plays some music when the robot has completed it's objective.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
@SuppressWarnings("unused")
|
@SuppressWarnings("unused")
|
||||||
public class VictoryBehavior extends BaseBehavior {
|
public class VictoryBehavior implements Behavior {
|
||||||
private static final int C = 262;
|
private static final int C = 262;
|
||||||
private static final int D = 287;
|
private static final int D = 287;
|
||||||
private static final int E = 320;
|
private static final int E = 320;
|
||||||
@@ -15,34 +36,64 @@ public class VictoryBehavior extends BaseBehavior {
|
|||||||
private static final int G = 392;
|
private static final int G = 392;
|
||||||
private static final int A = 440;
|
private static final int A = 440;
|
||||||
private static final int B = 494;
|
private static final int B = 494;
|
||||||
|
private static final int ROUND = 1000;
|
||||||
|
private static final int WHITE = 500;
|
||||||
|
private static final int BLACK = 250;
|
||||||
|
private static final int QUARTER = 125;
|
||||||
|
|
||||||
|
private Point[] score;
|
||||||
private RobotStateSingleton state;
|
private RobotStateSingleton state;
|
||||||
private SensorEventsQueue queue;
|
private SensorEventsQueue queue;
|
||||||
private FeatureDetectorsHandler detectorHandler;
|
private FeatureDetectorsHandler detectorHandler;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link VictoryBehavior}.
|
||||||
|
*/
|
||||||
public VictoryBehavior() {
|
public VictoryBehavior() {
|
||||||
super(null, null, null, null, null, 0.0f, 0.0f);
|
|
||||||
state = RobotStateSingleton.getInstance();
|
state = RobotStateSingleton.getInstance();
|
||||||
this.queue = SensorEventsQueue.getInstance();
|
this.queue = SensorEventsQueue.getInstance();
|
||||||
this.detectorHandler = FeatureDetectorsHandler.getInstance();
|
this.detectorHandler = FeatureDetectorsHandler.getInstance();
|
||||||
|
|
||||||
|
this.score = new Point[17];
|
||||||
|
score[0] = new Point(B, BLACK);
|
||||||
|
score[1] = new Point(A, BLACK);
|
||||||
|
score[2] = new Point(G, WHITE);
|
||||||
|
score[3] = new Point(B, BLACK);
|
||||||
|
score[4] = new Point(A, BLACK);
|
||||||
|
score[5] = new Point(G, WHITE);
|
||||||
|
score[6] = new Point(G, QUARTER);
|
||||||
|
score[7] = new Point(G, QUARTER);
|
||||||
|
score[8] = new Point(G, QUARTER);
|
||||||
|
score[9] = new Point(G, QUARTER);
|
||||||
|
score[10] = new Point(A, QUARTER);
|
||||||
|
score[11] = new Point(A, QUARTER);
|
||||||
|
score[12] = new Point(A, QUARTER);
|
||||||
|
score[13] = new Point(A, QUARTER);
|
||||||
|
score[14] = new Point(B, BLACK);
|
||||||
|
score[15] = new Point(A, BLACK);
|
||||||
|
score[16] = new Point(G, WHITE);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public boolean takeControl() {
|
public boolean takeControl() {
|
||||||
|
/* Check if the current state is VICTORY. */
|
||||||
if(state.getState() == States.VICTORY) {
|
if(state.getState() == States.VICTORY) {
|
||||||
detectorHandler.disableAllDetectors();
|
detectorHandler.disableAllDetectors();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* If the state is not VICTORY, then check if there is a light sensor
|
||||||
|
* event indicating that the goal line has been found. */
|
||||||
if(queue.hasNextLightSensorEvent()) {
|
if(queue.hasNextLightSensorEvent()) {
|
||||||
|
/* Set the state to VICTORY and disable sensors. */
|
||||||
state.setState(States.VICTORY);
|
state.setState(States.VICTORY);
|
||||||
detectorHandler.disableAllDetectors();
|
detectorHandler.disableAllDetectors();
|
||||||
|
|
||||||
|
/* Discard extraneous light events. */
|
||||||
while(queue.hasNextLightSensorEvent())
|
while(queue.hasNextLightSensorEvent())
|
||||||
queue.getNextLightSensorEvent();
|
queue.getNextLightSensorEvent();
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
@@ -50,30 +101,27 @@ public class VictoryBehavior extends BaseBehavior {
|
|||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void action() {
|
public void action() {
|
||||||
playToneDuration(B, 250);
|
/* Play some music! */
|
||||||
playToneDuration(A, 250);
|
playMusic(score);
|
||||||
playToneDuration(G, 500);
|
|
||||||
playToneDuration(B, 250);
|
/* Set the final state. */
|
||||||
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);
|
state.setState(States.DONE);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void suppress() { }
|
public void suppress() { }
|
||||||
|
|
||||||
|
private void playMusic(Point[] score) {
|
||||||
|
for(Point note: score)
|
||||||
|
playToneDuration(note.x, note.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Plays the given tone for the specified time given in milliseconds.
|
||||||
|
*
|
||||||
|
* @param tone
|
||||||
|
* @param milis
|
||||||
|
*/
|
||||||
private void playToneDuration(int tone, int milis) {
|
private void playToneDuration(int tone, int milis) {
|
||||||
Sound.playTone(tone, milis);
|
Sound.playTone(tone, milis);
|
||||||
try { Thread.sleep(milis); } catch(InterruptedException ie) {}
|
try { Thread.sleep(milis); } catch(InterruptedException ie) {}
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.behaviors;
|
||||||
|
|
||||||
import lejos.nxt.LightSensor;
|
import lejos.nxt.LightSensor;
|
||||||
@@ -9,10 +22,26 @@ 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.RobotStateSingleton;
|
||||||
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
import ve.ucv.ciens.cicore.icaro.ryabi.utils.States;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class implements a {@link BaseBehavior} that wanders in a straight line.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class WanderBehavior extends BaseBehavior {
|
public class WanderBehavior extends BaseBehavior {
|
||||||
private RobotStateSingleton state;
|
private RobotStateSingleton state;
|
||||||
private FeatureDetectorsHandler detectorHandler;
|
private FeatureDetectorsHandler detectorHandler;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Create a new {@link WanderBehavior}.
|
||||||
|
*
|
||||||
|
* @param pilot
|
||||||
|
* @param sonar
|
||||||
|
* @param touch
|
||||||
|
* @param light
|
||||||
|
* @param compass
|
||||||
|
* @param wheelDiameter
|
||||||
|
* @param trackWidth
|
||||||
|
*/
|
||||||
public WanderBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
|
public WanderBehavior(ArcRotateMoveController pilot, UltrasonicSensor sonar, TouchSensor touch, LightSensor light, CompassHTSensor compass, float wheelDiameter, float trackWidth) {
|
||||||
super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth);
|
super(pilot, sonar, touch, light, compass, wheelDiameter, trackWidth);
|
||||||
state = RobotStateSingleton.getInstance();
|
state = RobotStateSingleton.getInstance();
|
||||||
@@ -21,6 +50,7 @@ public class WanderBehavior extends BaseBehavior {
|
|||||||
|
|
||||||
@Override
|
@Override
|
||||||
public boolean takeControl() {
|
public boolean takeControl() {
|
||||||
|
/* Check if the state is wander. If it is, then set the sensors and return true. */
|
||||||
if(state.getState() == States.WANDER) {
|
if(state.getState() == States.WANDER) {
|
||||||
detectorHandler.enableTouchDetector();
|
detectorHandler.enableTouchDetector();
|
||||||
detectorHandler.enableLightDetector();
|
detectorHandler.enableLightDetector();
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
||||||
|
|
||||||
import lejos.nxt.Sound;
|
import lejos.nxt.Sound;
|
||||||
@@ -5,9 +18,18 @@ import lejos.robotics.objectdetection.Feature;
|
|||||||
import lejos.robotics.objectdetection.FeatureDetector;
|
import lejos.robotics.objectdetection.FeatureDetector;
|
||||||
import lejos.robotics.objectdetection.FeatureListener;
|
import lejos.robotics.objectdetection.FeatureListener;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A {@link FeatureListener} implementation that just plays a beep when any feature is detected and stores said feature at the
|
||||||
|
* global {@link SensorEventsQueue}.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class FeatureDetectionListener implements FeatureListener {
|
public class FeatureDetectionListener implements FeatureListener {
|
||||||
private SensorEventsQueue eventsQueue;
|
private SensorEventsQueue eventsQueue;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link FeatureDetectionListener}.
|
||||||
|
*/
|
||||||
public FeatureDetectionListener() {
|
public FeatureDetectionListener() {
|
||||||
this.eventsQueue = SensorEventsQueue.getInstance();
|
this.eventsQueue = SensorEventsQueue.getInstance();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,8 +1,28 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
||||||
|
|
||||||
import lejos.robotics.objectdetection.RangeFeatureDetector;
|
import lejos.robotics.objectdetection.RangeFeatureDetector;
|
||||||
import lejos.robotics.objectdetection.TouchFeatureDetector;
|
import lejos.robotics.objectdetection.TouchFeatureDetector;
|
||||||
|
import lejos.robotics.subsumption.Behavior;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Singleton class that holds all the feature detectors in one place so that the different {@link Behavior} implementations
|
||||||
|
* can operate on them.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public final class FeatureDetectorsHandler {
|
public final class FeatureDetectorsHandler {
|
||||||
private RangeFeatureDetector rangeDetector;
|
private RangeFeatureDetector rangeDetector;
|
||||||
private TouchFeatureDetector touchDetector;
|
private TouchFeatureDetector touchDetector;
|
||||||
@@ -18,6 +38,11 @@ public final class FeatureDetectorsHandler {
|
|||||||
private static final FeatureDetectorsHandler INSTANCE = new FeatureDetectorsHandler();
|
private static final FeatureDetectorsHandler INSTANCE = new FeatureDetectorsHandler();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Gets the singleton instance of this class.
|
||||||
|
*
|
||||||
|
* @return the instacne.
|
||||||
|
*/
|
||||||
public static FeatureDetectorsHandler getInstance() {
|
public static FeatureDetectorsHandler getInstance() {
|
||||||
return SingletonHolder.INSTANCE;
|
return SingletonHolder.INSTANCE;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
||||||
|
|
||||||
import lejos.geom.Point;
|
import lejos.geom.Point;
|
||||||
@@ -6,7 +19,39 @@ import lejos.robotics.RangeReading;
|
|||||||
import lejos.robotics.objectdetection.Feature;
|
import lejos.robotics.objectdetection.Feature;
|
||||||
import lejos.robotics.objectdetection.FeatureDetectorAdapter;
|
import lejos.robotics.objectdetection.FeatureDetectorAdapter;
|
||||||
import lejos.robotics.objectdetection.RangeFeature;
|
import lejos.robotics.objectdetection.RangeFeature;
|
||||||
|
import lejos.robotics.objectdetection.TouchFeatureDetector;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* <p>A {@link FeatureDetectorAdapter} subclass that reports events detected by the light sensor every 50 milliseconds. It is based
|
||||||
|
* on the {@link TouchFeatureDetector} class.</p>
|
||||||
|
*
|
||||||
|
* <p>The detector can take into account the position of the light sensor with respect to the rotation center of the robot.
|
||||||
|
* Use the following guide to calculate the x offset and y offset of the sensor:</p>
|
||||||
|
*
|
||||||
|
* <p><pre>+-----------------+</pre></p>
|
||||||
|
* <p><pre>| y----L |</pre></p>
|
||||||
|
* <p><pre>| | | |</pre></p>
|
||||||
|
* <p><pre>|W C----x W|</pre></p>
|
||||||
|
* <p><pre>| |</pre></p>
|
||||||
|
* <p><pre>| |</pre></p>
|
||||||
|
* <p><pre>| |</pre></p>
|
||||||
|
* <p><pre>| |</pre></p>
|
||||||
|
* <p><pre>| |</pre></p>
|
||||||
|
* <p><pre>+-----------------+</pre></p>
|
||||||
|
*
|
||||||
|
* <p>Where:</p>
|
||||||
|
*
|
||||||
|
* <ul>
|
||||||
|
* <li> C is the rotation center of the robot. </li>
|
||||||
|
* <li> W are the centers of the differential wheels of the robot. </li>
|
||||||
|
* <li> L is the light sensor. </li>
|
||||||
|
* <li> x and y are the distances from the rotation center to the light sensor taken along the respective edges of the robot. </li>
|
||||||
|
* </ul>
|
||||||
|
*
|
||||||
|
* <p>The front of the robot is at the top of the diagram.</p>
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class LightFeatureDetector extends FeatureDetectorAdapter {
|
public class LightFeatureDetector extends FeatureDetectorAdapter {
|
||||||
private static final int DELAY = 50;
|
private static final int DELAY = 50;
|
||||||
|
|
||||||
@@ -16,37 +61,111 @@ public class LightFeatureDetector extends FeatureDetectorAdapter {
|
|||||||
private float angle = 0;
|
private float angle = 0;
|
||||||
private float range = 0;
|
private float range = 0;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link LightFeatureDetector} using the given light sensor. The detection threshold is
|
||||||
|
* set to 50% and it assumes the sensor is at the rotation center of the robot. The detector will report events
|
||||||
|
* when the light measurement is less than 50%.
|
||||||
|
*
|
||||||
|
* @param lightSensor the light sensor to use.
|
||||||
|
*/
|
||||||
public LightFeatureDetector(LightSensor lightSensor) {
|
public LightFeatureDetector(LightSensor lightSensor) {
|
||||||
this(lightSensor, 50, false, 0, 0);
|
this(lightSensor, 50, false, 0, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link LightFeatureDetector} using the given light sensor and the given threshold. It assumes the sensor is at
|
||||||
|
* the rotation center of the robot. The detector will report events when the light measurement is less than the threshold.
|
||||||
|
*
|
||||||
|
* @param lightSensor the light sensor to use.
|
||||||
|
* @param threshold the detection threshold. It will be clamped to [0, 100].
|
||||||
|
*/
|
||||||
public LightFeatureDetector(LightSensor lightSensor, int threshold) {
|
public LightFeatureDetector(LightSensor lightSensor, int threshold) {
|
||||||
this(lightSensor, threshold, false, 0, 0);
|
this(lightSensor, threshold, false, 0, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link LightFeatureDetector} using the given light sensor. The detection threshold is
|
||||||
|
* set to 50% and it assumes the sensor is at the rotation center of the robot. The detector will report events
|
||||||
|
* when the light measurement is more than or equal to 50% if invert is true.
|
||||||
|
*
|
||||||
|
* @param lightSensor the light sensor to use.
|
||||||
|
* @param invert
|
||||||
|
*/
|
||||||
public LightFeatureDetector(LightSensor lightSensor, boolean invert) {
|
public LightFeatureDetector(LightSensor lightSensor, boolean invert) {
|
||||||
this(lightSensor, 50, invert, 0, 0);
|
this(lightSensor, 50, invert, 0, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link LightFeatureDetector} using the given light sensor. The detection threshold is
|
||||||
|
* set to 50%. The offsets determine the position of the sensor with respect to the center of the robot.
|
||||||
|
* The detector will report events when the light measurement is less than 50%.
|
||||||
|
*
|
||||||
|
* To calculate the offsets use the following guide:
|
||||||
|
*
|
||||||
|
* @param lightSensor the light sensor to use.
|
||||||
|
* @param xOffset distance of the sensor to the rotation center of the robot.
|
||||||
|
* @param yOffset distance of the sensor to the rotation center of the robot.
|
||||||
|
*/
|
||||||
public LightFeatureDetector(LightSensor lightSensor, double xOffset, double yOffset) {
|
public LightFeatureDetector(LightSensor lightSensor, double xOffset, double yOffset) {
|
||||||
this(lightSensor, 50, false, xOffset, yOffset);
|
this(lightSensor, 50, false, xOffset, yOffset);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link LightFeatureDetector} using the given light sensor. The detection threshold is
|
||||||
|
* set to the given threshold. The offsets determine the position of the sensor with respect to the center of the robot.
|
||||||
|
* The detector will report events when the light measurement is less than the given threshold.
|
||||||
|
*
|
||||||
|
* To calculate the offsets use the following guide:
|
||||||
|
*
|
||||||
|
* @param lightSensor the light sensor to use.
|
||||||
|
* @param threshold the detection threshold. It will be clamped to [0, 100].
|
||||||
|
* @param xOffset distance of the sensor to the rotation center of the robot.
|
||||||
|
* @param yOffset distance of the sensor to the rotation center of the robot.
|
||||||
|
*/
|
||||||
public LightFeatureDetector(LightSensor lightSensor, int threshold, double xOffset, double yOffset) {
|
public LightFeatureDetector(LightSensor lightSensor, int threshold, double xOffset, double yOffset) {
|
||||||
this(lightSensor, threshold, false, xOffset, yOffset);
|
this(lightSensor, threshold, false, xOffset, yOffset);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link LightFeatureDetector} using the given light sensor. The detection threshold is
|
||||||
|
* set to 50%. The offsets determine the position of the sensor with respect to the center of the robot.
|
||||||
|
* The detector will report events when the light measurement is more than or equal to 50% if invert is true.
|
||||||
|
*
|
||||||
|
* To calculate the offsets use the following guide:
|
||||||
|
*
|
||||||
|
* @param lightSensor the light sensor to use.
|
||||||
|
* @param invert
|
||||||
|
* @param xOffset distance of the sensor to the rotation center of the robot.
|
||||||
|
* @param yOffset distance of the sensor to the rotation center of the robot.
|
||||||
|
*/
|
||||||
public LightFeatureDetector(LightSensor lightSensor, boolean invert, double xOffset, double yOffset) {
|
public LightFeatureDetector(LightSensor lightSensor, boolean invert, double xOffset, double yOffset) {
|
||||||
this(lightSensor, 50, invert, xOffset, yOffset);
|
this(lightSensor, 50, invert, xOffset, yOffset);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link LightFeatureDetector} using the given light sensor. The detection threshold is
|
||||||
|
* set to the given threshold. The offsets determine the position of the sensor with respect to the center of the robot.
|
||||||
|
* The detector will report events when the light measurement is more than the given threshold if invert is true.
|
||||||
|
*
|
||||||
|
* To calculate the offsets use the following guide:
|
||||||
|
*
|
||||||
|
* @param lightSensor the light sensor to use.
|
||||||
|
* @param threshold the detection threshold. It will be clamped to [0, 100].
|
||||||
|
* @param invert
|
||||||
|
* @param xOffset distance of the sensor to the rotation center of the robot.
|
||||||
|
* @param yOffset distance of the sensor to the rotation center of the robot.
|
||||||
|
*/
|
||||||
public LightFeatureDetector(LightSensor lightSensor, int threshold, boolean invert, double xOffset, double yOffset) {
|
public LightFeatureDetector(LightSensor lightSensor, int threshold, boolean invert, double xOffset, double yOffset) {
|
||||||
super(DELAY);
|
super(DELAY);
|
||||||
this.lightSensor = lightSensor;
|
this.lightSensor = lightSensor;
|
||||||
this.threshold = threshold;
|
this.threshold = threshold;
|
||||||
this.invert = invert;
|
this.invert = invert;
|
||||||
|
|
||||||
// Calculate angle and distance of light sensor from center:
|
/* Clamp the detection threshold. */
|
||||||
|
this.threshold = (this.threshold > 100) ? 100 : this.threshold;
|
||||||
|
this.threshold = (this.threshold < 0) ? 0 : this.threshold;
|
||||||
|
|
||||||
|
/* Calculate angle and distance of light sensor from center. */
|
||||||
Point robot_center = new Point(0, 0);
|
Point robot_center = new Point(0, 0);
|
||||||
Point bumper_p = new Point((float)xOffset, (float)yOffset);
|
Point bumper_p = new Point((float)xOffset, (float)yOffset);
|
||||||
range = (float)robot_center.distance(xOffset, yOffset);
|
range = (float)robot_center.distance(xOffset, yOffset);
|
||||||
@@ -56,6 +175,9 @@ public class LightFeatureDetector extends FeatureDetectorAdapter {
|
|||||||
@Override
|
@Override
|
||||||
public Feature scan() {
|
public Feature scan() {
|
||||||
RangeFeature rf = null;
|
RangeFeature rf = null;
|
||||||
|
|
||||||
|
/* Calculate the distance and angle of the feature and return it. Take into
|
||||||
|
* account if the detection factor is inverted. */
|
||||||
if(invert) {
|
if(invert) {
|
||||||
if(lightSensor.getLightValue() >= threshold) {
|
if(lightSensor.getLightValue() >= threshold) {
|
||||||
RangeReading rr = new RangeReading(angle, range);
|
RangeReading rr = new RangeReading(angle, range);
|
||||||
@@ -74,6 +196,8 @@ public class LightFeatureDetector extends FeatureDetectorAdapter {
|
|||||||
protected void notifyListeners(Feature feature) {
|
protected void notifyListeners(Feature feature) {
|
||||||
super.notifyListeners(feature);
|
super.notifyListeners(feature);
|
||||||
|
|
||||||
|
/* Stall the detector until the detection ends to avoid notifying the listeners
|
||||||
|
* multiple times for the same feature. */
|
||||||
if(invert) {
|
if(invert) {
|
||||||
while(lightSensor.getLightValue() >= threshold);
|
while(lightSensor.getLightValue() >= threshold);
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -1,12 +1,43 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
||||||
|
|
||||||
import lejos.robotics.objectdetection.Feature;
|
import lejos.robotics.objectdetection.Feature;
|
||||||
import lejos.robotics.objectdetection.FeatureDetector;
|
import lejos.robotics.objectdetection.FeatureDetector;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Container class that aggregates a detected {@link Feature} and the {@link FeatureDetector} that found it.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class SensorEvent {
|
public class SensorEvent {
|
||||||
|
/**
|
||||||
|
* A detected feature.
|
||||||
|
*/
|
||||||
public Feature feature;
|
public Feature feature;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The detector responsible for finding the feature.
|
||||||
|
*/
|
||||||
public FeatureDetector detector;
|
public FeatureDetector detector;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Commodity constructor.
|
||||||
|
*
|
||||||
|
* @param feature the detected feature.
|
||||||
|
* @param detector the respective detector.
|
||||||
|
*/
|
||||||
public SensorEvent(Feature feature, FeatureDetector detector) {
|
public SensorEvent(Feature feature, FeatureDetector detector) {
|
||||||
this.feature = feature;
|
this.feature = feature;
|
||||||
this.detector = detector;
|
this.detector = detector;
|
||||||
|
|||||||
@@ -1,3 +1,16 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
package ve.ucv.ciens.cicore.icaro.ryabi.sensors;
|
||||||
|
|
||||||
import java.util.Queue;
|
import java.util.Queue;
|
||||||
@@ -7,6 +20,11 @@ import lejos.robotics.objectdetection.FeatureDetector;
|
|||||||
import lejos.robotics.objectdetection.RangeFeatureDetector;
|
import lejos.robotics.objectdetection.RangeFeatureDetector;
|
||||||
import lejos.robotics.objectdetection.TouchFeatureDetector;
|
import lejos.robotics.objectdetection.TouchFeatureDetector;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A singleton {@link Queue} holder that keeps track of the different events detected by the sensors in FIFO order.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public final class SensorEventsQueue {
|
public final class SensorEventsQueue {
|
||||||
private Queue<SensorEvent> touchEventsQueue;
|
private Queue<SensorEvent> touchEventsQueue;
|
||||||
private Queue<SensorEvent> lightEventsQueue;
|
private Queue<SensorEvent> lightEventsQueue;
|
||||||
@@ -22,10 +40,21 @@ public final class SensorEventsQueue {
|
|||||||
private static final SensorEventsQueue INSTANCE = new SensorEventsQueue();
|
private static final SensorEventsQueue INSTANCE = new SensorEventsQueue();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Gets the singleton instance of this class.
|
||||||
|
*
|
||||||
|
* @return the instance.
|
||||||
|
*/
|
||||||
public static SensorEventsQueue getInstance() {
|
public static SensorEventsQueue getInstance() {
|
||||||
return SingletonHolder.INSTANCE;
|
return SingletonHolder.INSTANCE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Adds a new event to the respective queue based on it's type.
|
||||||
|
*
|
||||||
|
* @param feature the detected feature.
|
||||||
|
* @param detector the respective detector.
|
||||||
|
*/
|
||||||
public synchronized void addEvent(Feature feature, FeatureDetector detector) {
|
public synchronized void addEvent(Feature feature, FeatureDetector detector) {
|
||||||
SensorEvent event = new SensorEvent(feature, detector);
|
SensorEvent event = new SensorEvent(feature, detector);
|
||||||
|
|
||||||
|
|||||||
@@ -1,29 +1,59 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
|
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
|
||||||
|
|
||||||
import lejos.nxt.Button;
|
import lejos.nxt.Button;
|
||||||
import lejos.nxt.ButtonListener;
|
import lejos.nxt.ButtonListener;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class implements a {@link ButtonListener} that force-quits the program if the escape button of the robot
|
||||||
|
* has been pressed for 3 seconds or more.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public class QuitButtonListener implements ButtonListener {
|
public class QuitButtonListener implements ButtonListener {
|
||||||
TimeCounter counter;
|
private TimeCounter counter;
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void buttonPressed(Button b) {
|
public void buttonPressed(Button b) {
|
||||||
|
/* Start counting the time as soon as the button is pressed. */
|
||||||
counter = new TimeCounter();
|
counter = new TimeCounter();
|
||||||
counter.start();
|
counter.start();
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void buttonReleased(Button b) {
|
public void buttonReleased(Button b) {
|
||||||
|
/* Stop counting as soon as the button is released. */
|
||||||
if(counter != null) {
|
if(counter != null) {
|
||||||
counter.finish();
|
counter.finish();
|
||||||
counter = null;
|
counter = null;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This {@link Thread} counts time in intervals of 100 milliseconds and stops the program if
|
||||||
|
* the counted time exceeds 3000 milliseconds.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.*
|
||||||
|
*/
|
||||||
class TimeCounter extends Thread {
|
class TimeCounter extends Thread {
|
||||||
private boolean done;
|
private boolean done;
|
||||||
private long timeMilisBefore;
|
private long timeMilisBefore;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a new {@link TimeCounter}
|
||||||
|
*/
|
||||||
public TimeCounter() {
|
public TimeCounter() {
|
||||||
done = false;
|
done = false;
|
||||||
}
|
}
|
||||||
@@ -32,16 +62,24 @@ public class QuitButtonListener implements ButtonListener {
|
|||||||
public void run() {
|
public void run() {
|
||||||
long timeMilisNow;
|
long timeMilisNow;
|
||||||
|
|
||||||
|
/* Get the time before counting. */
|
||||||
timeMilisBefore = System.currentTimeMillis();
|
timeMilisBefore = System.currentTimeMillis();
|
||||||
|
|
||||||
|
/* Start counting. */
|
||||||
while(!done) {
|
while(!done) {
|
||||||
|
/* Sleep for 100 milliseconds. and then check the time. */
|
||||||
try { Thread.sleep(100); } catch (InterruptedException e) { }
|
try { Thread.sleep(100); } catch (InterruptedException e) { }
|
||||||
timeMilisNow = System.currentTimeMillis();
|
timeMilisNow = System.currentTimeMillis();
|
||||||
|
|
||||||
|
/* If the time delta since counting started is greater than 3 seconds then force quit. */
|
||||||
if(timeMilisNow - timeMilisBefore > 3000)
|
if(timeMilisNow - timeMilisBefore > 3000)
|
||||||
System.exit(0);
|
System.exit(0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Stops the counting loop.
|
||||||
|
*/
|
||||||
public void finish() {
|
public void finish() {
|
||||||
done = true;
|
done = true;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,5 +1,23 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
|
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Singleton class that holds the current state of the robot's goal. See {@link States}.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public final class RobotStateSingleton {
|
public final class RobotStateSingleton {
|
||||||
private States state;
|
private States state;
|
||||||
|
|
||||||
@@ -11,14 +29,29 @@ public final class RobotStateSingleton {
|
|||||||
private static final RobotStateSingleton INSTANCE = new RobotStateSingleton();
|
private static final RobotStateSingleton INSTANCE = new RobotStateSingleton();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Returns the singleton instance of this class.
|
||||||
|
*
|
||||||
|
* @return the instance.
|
||||||
|
*/
|
||||||
public static RobotStateSingleton getInstance() {
|
public static RobotStateSingleton getInstance() {
|
||||||
return SingletonHolder.INSTANCE;
|
return SingletonHolder.INSTANCE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Changes the state of the robot.
|
||||||
|
*
|
||||||
|
* @param state the state to set.
|
||||||
|
*/
|
||||||
public synchronized void setState(States state) {
|
public synchronized void setState(States state) {
|
||||||
this.state = state;
|
this.state = state;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Returns the current state of the robot.
|
||||||
|
*
|
||||||
|
* @return the current state.
|
||||||
|
*/
|
||||||
public States getState() {
|
public States getState() {
|
||||||
return this.state;
|
return this.state;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,17 +1,43 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
|
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
|
||||||
|
|
||||||
import lejos.nxt.addon.CompassHTSensor;
|
import lejos.nxt.addon.CompassHTSensor;
|
||||||
import lejos.robotics.navigation.ArcRotateMoveController;
|
import lejos.robotics.navigation.ArcRotateMoveController;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Helper class that allows the robot to make 90 degrees turns to the left and right and 180 degrees turns.
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public abstract class Rotations {
|
public abstract class Rotations {
|
||||||
|
/**
|
||||||
|
* Rotates the robot 90 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 rotate90(CompassHTSensor compass, ArcRotateMoveController pilot) {
|
public static void rotate90(CompassHTSensor compass, ArcRotateMoveController pilot) {
|
||||||
float cMeasure;
|
float cMeasure;
|
||||||
|
|
||||||
|
/* First reset the compass. */
|
||||||
compass.resetCartesianZero();
|
compass.resetCartesianZero();
|
||||||
|
|
||||||
|
/* Start rotating slowly. */
|
||||||
pilot.setRotateSpeed(25);
|
pilot.setRotateSpeed(25);
|
||||||
pilot.rotate(Integer.MIN_VALUE - 1, true);
|
pilot.rotate(Integer.MIN_VALUE - 1, true);
|
||||||
|
|
||||||
|
/* Keep rotating until the robot has turned 90 degrees. */
|
||||||
cMeasure = 360.0f;
|
cMeasure = 360.0f;
|
||||||
try { Thread.sleep(200); } catch (InterruptedException e) { }
|
try { Thread.sleep(200); } catch (InterruptedException e) { }
|
||||||
while(cMeasure > 270.0f) {
|
while(cMeasure > 270.0f) {
|
||||||
@@ -20,14 +46,23 @@ public abstract class Rotations {
|
|||||||
pilot.stop();
|
pilot.stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Rotates the robot 90 degrees counterclockwise.
|
||||||
|
*
|
||||||
|
* @param compass A {@link CompassHTSensor} to use to make good turns.
|
||||||
|
* @param pilot The {@link ArcRotateMoveController} implementation that controls the robot.
|
||||||
|
*/
|
||||||
public static void rotateM90(CompassHTSensor compass, ArcRotateMoveController pilot) {
|
public static void rotateM90(CompassHTSensor compass, ArcRotateMoveController pilot) {
|
||||||
float cMeasure;
|
float cMeasure;
|
||||||
|
|
||||||
|
/* First reset the compass. */
|
||||||
compass.resetCartesianZero();
|
compass.resetCartesianZero();
|
||||||
|
|
||||||
|
/* Start rotating slowly. */
|
||||||
pilot.setRotateSpeed(25);
|
pilot.setRotateSpeed(25);
|
||||||
pilot.rotate(-3000, true);
|
pilot.rotate(-3000, true);
|
||||||
|
|
||||||
|
/* Keep rotating until the robot has turned 90 degrees. */
|
||||||
cMeasure = 0.0f;
|
cMeasure = 0.0f;
|
||||||
try { Thread.sleep(200); } catch (InterruptedException e) { }
|
try { Thread.sleep(200); } catch (InterruptedException e) { }
|
||||||
while(cMeasure < 90.0f) {
|
while(cMeasure < 90.0f) {
|
||||||
@@ -36,14 +71,23 @@ public abstract class Rotations {
|
|||||||
pilot.stop();
|
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) {
|
public static void rotate180(CompassHTSensor compass, ArcRotateMoveController pilot) {
|
||||||
float cMeasure;
|
float cMeasure;
|
||||||
|
|
||||||
|
/* First reset the compass. */
|
||||||
compass.resetCartesianZero();
|
compass.resetCartesianZero();
|
||||||
|
|
||||||
|
/* Start rotating slowly. */
|
||||||
pilot.setRotateSpeed(25);
|
pilot.setRotateSpeed(25);
|
||||||
pilot.rotate(Integer.MIN_VALUE - 1, true);
|
pilot.rotate(Integer.MIN_VALUE - 1, true);
|
||||||
|
|
||||||
|
/* Keep rotating until the robot has turned 90 degrees. */
|
||||||
cMeasure = 360.0f;
|
cMeasure = 360.0f;
|
||||||
try { Thread.sleep(200); } catch (InterruptedException e) { }
|
try { Thread.sleep(200); } catch (InterruptedException e) { }
|
||||||
while(cMeasure > 180.0f) {
|
while(cMeasure > 180.0f) {
|
||||||
|
|||||||
@@ -1,5 +1,36 @@
|
|||||||
|
/*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* See the LICENSE file for more details.
|
||||||
|
*/
|
||||||
|
|
||||||
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
|
package ve.ucv.ciens.cicore.icaro.ryabi.utils;
|
||||||
|
|
||||||
|
import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.CatchBallBehavior;
|
||||||
|
import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.SearchBallBehavior;
|
||||||
|
import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.VictoryBehavior;
|
||||||
|
import ve.ucv.ciens.cicore.icaro.ryabi.behaviors.WanderBehavior;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This enum defines the valid robot states. The allowed states are as follows.
|
||||||
|
*
|
||||||
|
* <ul>
|
||||||
|
* <li> WANDER: see {@link WanderBehavior}. </li>
|
||||||
|
* <li> SEARCH_BALL: see {@link SearchBallBehavior}. </li>
|
||||||
|
* <li> BALL_FOUND: see {@link CatchBallBehavior}. </li>
|
||||||
|
* <li> VICTORY: see {@link VictoryBehavior}. </li>
|
||||||
|
* <li> DONE: no behavior is active and the program ends. </li>
|
||||||
|
* </ul>
|
||||||
|
*
|
||||||
|
* @author Miguel Angel Astor Romero.
|
||||||
|
*/
|
||||||
public enum States {
|
public enum States {
|
||||||
WANDER, SEARCH_BALL, BALL_FOUND, VICTORY, DONE;
|
WANDER, SEARCH_BALL, BALL_FOUND, VICTORY, DONE;
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user