Commit a711a3a4 authored by Peder Kronsgaard Detlefsen's avatar Peder Kronsgaard Detlefsen
Browse files

Added ArenaRobot and initial PC code.

parent daa2fd99
/**
* Arbitrator controls which Behavior object will become active in
* a behavior control system.
*
* Make sure to call start() after the Arbitrator is instantiated.<br>
*
* This class has three major responsibilities: <br>
* 1. Determine the highest priority behavior that returns <b> true </b> to takeControl()<br>
* 2. Suppress the active behavior if its priority is less than highest priority. <br>
* 3. When the action() method exits, call action() on the Behavior of highest priority.
* <br> The Arbitrator assumes that a Behavior is no longer active when action() exits,
* <br> therefore it will only call suppress() on the Behavior whose action() method is running.
* <br> It can make consecutive calls of action() on the same Behavior.
* <br> Requirements for a Behavior:
* <br> When suppress() is called, terminate action() immediately.
* <br> When action() exits, the robot is in a safe state (e.g. motors stopped)
* @see Behavior
* @author Roger Glassey
*
* Modified so the Behavior with the highest integer priority returned by
* takeControl is the active behavior. Also modified so start() never exits,
* there should always be a behavior that wants to be active.
*
* Ole Caprani, 24-12-2012
*
*/
public class Arbitrator
{
private final int NONE = -1;
private Behavior[] behavior;
private BehaviorAction actionThread;
private int currentBehavior = NONE;
private int currentPriority = NONE;
public Arbitrator(Behavior[] behaviorList)
{
behavior = behaviorList;
actionThread = new BehaviorAction();
actionThread.setDaemon(true);
}
/**
* This method starts the arbitration of Behaviors and runs an endless loop.
* The start() method will never return
*/
public void start()
{
int highest, maxPriority;
actionThread.start();
while (true)
{
// Find behavior with highest priority
maxPriority = -1; highest = -1;
for (int i = 0; i < behavior.length; i++)
{
int priority = behavior[i].takeControl();
if (priority > maxPriority )
{
highest = i;
maxPriority = priority;
}
}
// Start highest priority process and update currentPriority
if ( actionThread.current == NONE)
{
currentBehavior = highest;
currentPriority = maxPriority;
actionThread.execute(highest);
}
else
if ( currentPriority < maxPriority )
{
behavior[currentBehavior].suppress();
currentBehavior = highest;
currentPriority = maxPriority;
actionThread.execute(highest);
}
else
currentPriority = maxPriority;
Thread.yield();
}
}
/**
* Local thread that runs the action method for the currently
* highest priority behavior
*/
private class BehaviorAction extends Thread
{
public int current = NONE;
public void run()
{
while (true)
{
synchronized (this)
{
if ( current != NONE )
{
behavior[current].action();
current = NONE;
}
}
Thread.yield();
}
}
public synchronized void execute(int index)
{
current = index;
}
}
}
import lejos.nxt.*;
import lejos.nxt.comm.RConsole;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
import lejos.util.Delay;
public class ArenaRobot {
double mult = 1.007;
double leftWheelDiameter = 55 * mult, rightWheelDiameter = 54.84 * mult,
trackWidth = 165;
double travelSpeed = 100, rotateSpeed = 100;
NXTRegulatedMotor left = Motor.A;
NXTRegulatedMotor right = Motor.C;
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter,
rightWheelDiameter, trackWidth, left, right, false);
OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
public ArenaRobot() {
pilot.setTravelSpeed(travelSpeed);
pilot.setRotateSpeed(rotateSpeed);
Pose initialPose = new Pose(0,0,0);
RConsole.open();
poseProvider.setPose(initialPose);
}
public void travel(double distance) {
pilot.travel(distance, true);
}
public void rotate(double angle) {
pilot.rotate(angle, true);
}
public void show(Pose p) {
LCD.clear();
LCD.drawString("Pose X " + p.getX(), 0, 2);
LCD.drawString("Pose Y " + p.getY(), 0, 3);
LCD.drawString("Pose V " + p.getHeading(), 0, 4);
}
public static void main(String[] args) {
ArenaRobot pilot = new ArenaRobot();
LCD.clear();
LCD.drawString("ArenaRobot", 0, 0);
Sound.beep();
while (!Button.ENTER.isDown())
Thread.yield();
Sound.twoBeeps();
Behavior b1 = new Wander(pilot);
Behavior b2 = new AvoidEdge(pilot);
Behavior b3 = new Exit(pilot);
Behavior[] behaviorList = {b1, b2, b3};
Arbitrator arbitrator = new Arbitrator(behaviorList);
arbitrator.start();
}
}
class Wander implements Behavior {
private static final int TRAVEL = 0;
private static final int ROTATE = 1;
private boolean _suppressed = false;
private ArenaRobot pilot;
public Wander(ArenaRobot pilot) {
this.pilot = pilot;
}
public int takeControl() {
return 10; // this behavior always wants control.
}
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
public void action() {
_suppressed = false;
double rand = Math.random();
if (rand > 0.5) {
pilot.travel(rand * 2000);
} else {
pilot.rotate((Math.random() > 0.5) ? rand * 180 : rand * -180);
}
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield(); // don't exit till suppressed
}
pilot.show(pilot.poseProvider.getPose());
Motor.A.stop(true); // not strictly necessary, but good programming practice
Motor.C.stop(true);
}
}
class AvoidEdge extends Thread implements Behavior {
LightSensor light = new LightSensor(SensorPort.S1);
TouchSensor touch = new TouchSensor(SensorPort.S2);
private int lightVal = 100;
private boolean _suppressed = false;
private ArenaRobot pilot;
public AvoidEdge(ArenaRobot pilot) {
this.pilot = pilot;
this.isDaemon();
this.start();
}
public void run() {
while (true) {
lightVal = light.getLightValue();
}
}
public int takeControl() {
return (lightVal < 40 || touch.isPressed()) ? 20 : 0;
}
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
public void action() {
_suppressed = false;
Sound.buzz();
// Take evasive action.
pilot.travel(-200);
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield(); // don't exit till suppressed
}
pilot.show(pilot.poseProvider.getPose());
double rand = Math.random();
double angle = (rand > 0.5) ? 90 + (90 * rand) : -90 - (90 * rand);
pilot.rotate(angle);
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield(); // don't exit till suppressed
}
pilot.show(pilot.poseProvider.getPose());
Motor.A.stop(true); // not strictly necessary, but good programming practice
Motor.C.stop(true);
}
}
class Exit implements Behavior {
private boolean _suppressed = false;
private ArenaRobot pilot;
public Exit(ArenaRobot pilot) {
this.pilot = pilot;
}
public int takeControl() {
return (Button.ESCAPE.isDown()) ? 100 : 0 ; // this behavior always wants control.
}
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
public void action() {
_suppressed = false;
pilot.show(pilot.poseProvider.getPose());
Motor.A.stop(true); // not strictly necessary, but good programming practice
Motor.C.stop(true);
Delay.msDelay(2000);
System.exit(0);
}
}
\ No newline at end of file
/**
* The Behavior interface represents an object embodying a specific
* behavior belonging to a robot. Each behavior must define three things: <BR>
* 1) The circumstances to make this behavior seize control of the robot.
* e.g. When the touch sensor determines the robot has collided with an object.<BR>
* 2) The action to perform when this behavior takes control.
* e.g. Back up and turn.<BR>
* 3) A way to quickly exit from the action when the Arbitrator selects a higher
* priority behavior to take control.
* These are represented by defining the methods takeControl(), action(),
* and suppress() respectively. <BR>
* A behavior control system has one or more Behavior objects. When you have defined
* these objects, create an array of them and use that array to initialize an
* Arbitrator object.
*
* @see Arbitrator
* @version 0.9 May 2011
* Modified so takeControl returns an integer, Ole Caprani 24-12-2012
*/
public interface Behavior {
/**
* The integer returned indicates how much this behavior wants control of the robot.
* For example, a robot that reacts if a touch sensor is pressed: <BR>
* public int takeControl() { <BR>
* if ( touch.isPressed() ) return 100;
* return 0; <BR>
* } <BR>
* @return integer Indicates if this Behavior should seize control.
*/
public int takeControl();
/**
* The code in action() represents the tasks the robot performs when this
* behavior becomes active. It can be as complex as navigating around a
* room, or as simple as playing a tune.<BR>
* <B>The contract for implementing this method is:</B><BR>
* If its task is is complete, the method returns.
* It also <B> must </B> return promptly when the suppress() method
* is called, for example by testing the boolean suppress flag. <br>
* When this method exits, the robot is in a safe state for another behavior
* to run its action() method
*/
public void action();
/**
* The code in suppress() should cause the current behavior to exit. <BR>
* <B>The contract for implementing this method is:</B><BR>
* Exit quickly, for example, just set boolean flag.
*/
public void suppress();
}
\ No newline at end of file
import java.awt.Color;
import lejos.robotics.navigation.Pose;
public class Map
{
// The map of the robot environment
int dimX, dimY, width, rangeX, rangeY;
private Color[][] map;
public Map(int dimX, int dimY, int width)
{
this.dimX = dimX;
this.dimY = dimY;
this.width = width;
rangeX = dimX*width;
rangeY = dimY*width;
map = new Color[dimX][dimY];
for (int i = 0; i < dimX; i++)
for (int j = 0; j < dimY; j++) map[i][j] = Color.WHITE;
}
public void setColor(int i, int j, Color c)
{
map[i][j] = c;
}
public Color getColor(int i, int j)
{
return (map[i][j]);
}
public Color getColor(Pose p)
{
float x = p.getX(), y = p.getY();
if ( x < 0 || x > rangeX || y < 0 || y > rangeY )
return Color.ORANGE;
else
return map[(int)(x/width)][(int)(y/width)];
}
public int getWidth()
{
return (width);
}
public int getDimX()
{
return (dimX);
}
public int getDimY()
{
return (dimY);
}
}
import java.util.Random;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
import lejos.geom.*;
import java.awt.Color;
/**
* Represents a particle for the particle filtering algorithm. The state of the
* particle is the pose, which represents a possible pose of the robot.
* The weight represents the relative probability that the robot has this
* pose. Weights are from 0 to 1.
*
* This is a version of the leJOS class MCLParticle with a light sensor used to
* detect black or white tiles in a 2D map of black/white tiles.
*
* @author Ole Caprani
* @version 22.05.15
*
*
*/
public class Particle
{
private static Random rand = new Random();
private Pose pose;
private float weight = 1;
private int blackWhiteThreshold = 500;
/**
* Create a particle with a specific pose
*
* @param pose the pose
*/
public Particle(Pose pose) {
this.pose = pose;
}
/**
* Set the weight for this particle
*
* @param weight the weight of this particle
*/
public void setWeight(float weight) {
this.weight = weight;
}
/**
* Return the weight of this particle
*
* @return the weight
*/
public float getWeight()
{
return weight;
}
/**
* Return the pose of this particle
*
* @return the pose
*/
public Pose getPose()
{
return pose;
}
/**
* Calculate the weight for this particle by comparing the reading in this
* position as seen on the map with the
* robot's light sensor reading.
*
*/
public void calculateWeight(int lightValue, Map m)
{
if ( m.getColor(pose) == Color.BLACK )
{
if ( lightValue > blackWhiteThreshold)
weight = 0.9f;
else
weight = 0.1f;
}
else
{
if ( m.getColor(pose) == Color.WHITE )
{
if ( lightValue > blackWhiteThreshold)
weight = 0.1f;
else
weight = 0.9f;
}
else // outside the map
weight = 0.0f;
}
}
/**
* Apply the robot's move to the particle with a bit of random noise.
* Only works for rotate or travel movements.
*
* @param move the robot's move
*/
public void applyMove(Move move, float distanceNoiseFactor, float angleNoiseFactor)
{
float ym = (move.getDistanceTraveled() * ((float) Math.sin(Math.toRadians(pose.getHeading()))));
float xm = (move.getDistanceTraveled() * ((float) Math.cos(Math.toRadians(pose.getHeading()))));
pose.setLocation(new Point(
(float) (pose.getX() + xm + (distanceNoiseFactor * xm * rand.nextGaussian())),
(float) (pose.getY() + ym + (distanceNoiseFactor * ym * rand.nextGaussian()))));
pose.setHeading(
(float) (pose.getHeading() + move.getAngleTurned() + (angleNoiseFactor * rand.nextGaussian())));
pose.setHeading((float) ((int) (pose.getHeading() + 0.5f) % 360));
}
}
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;
import java.awt.Rectangle;
/**
* Represents a particle set for the particle filtering algorithm.
*
* This is a version of the leJOS class MCLParticles with the particles
* generated within a 2D map of black/white tiles.
*
* @author Ole Caprani
* @version 22.05.15
*
*/
public class ParticleSet
{
// Constants
private static final float BIG_FLOAT = 10000f;
// Static variables
public static int maxIterations = 100;
// Instance variables
private float distanceNoiseFactor = 0.02f;
private float angleNoiseFactor = 1f;
private int numParticles;
private Particle[] particles;
private Map map;
private int _iterations;
private float _x, _y, _heading;
private float minX, maxX, minY, maxY;
private double varX, varY, varH;
/**
* Returns the best best estimate of the current pose;
* @return the estimated pose
*/
public Pose getPose()
{
estimatePose();
return new Pose(_x, _y, _heading);
}
/**
* Create a set of particles.
*
* @param map the map of the enclosed environment
*/
public ParticleSet(int numParticles, Map m)
{
this.numParticles = numParticles;
map = m;
particles = new Particle[numParticles];
for (int i = 0; i < numParticles; i++)
{
particles[i] = generateParticle(map);
}
}
private Particle generateParticle(Map m)
{
int sizeX = m.getDimX()*m.getWidth();
int sizeY = m.getDimY()*m.getWidth();
// Generate a particle with a location (x,y) randomly chosen within the
// 2D area of the map. The heading can be chosen as suggested in several
// different ways.
Particle p = new Particle(new Pose(
(float)(Math.random()*sizeX), (float)(Math.random()*sizeY),
//(float)(Math.random()*360)));
(float)((int)(Math.random()*2)*180)));
//0));
p.setWeight(1);