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

Almost complete solution. Avoid behavior is missing.

parent 5e9feeea
/**
* 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;
}
}
}
/**
* 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 lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move;
public class PilotAvoid {
private SlaveIOStreams PC;
double mult = 1.007;
double leftWheelDiameter = 55 * mult, rightWheelDiameter = 54.84 * mult,
trackWidth = 165;
double travelSpeed = 50, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
NXTRegulatedMotor right = Motor.C;
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter,
rightWheelDiameter, trackWidth, left, right, false);
public PilotAvoid(boolean usb) {
PC = new SlaveIOStreams(usb);
PC.open();
pilot.setTravelSpeed(travelSpeed);
pilot.setRotateSpeed(rotateSpeed);
}
public void sendMove(Move move) {
PC.output((move.getMoveType() == Move.MoveType.TRAVEL ? 0 : 1));
PC.output(move.getDistanceTraveled());
PC.output(move.getAngleTurned());
}
public void travel(double distance) {
pilot.travel(distance, true);
}
public void rotate(double angle) {
pilot.rotate(angle, true);
}
public void go() {
Sound.beep();
while (!Button.ENTER.isDown())
Thread.yield();
Sound.twoBeeps();
travel(500);
rotate(90);
travel(200);
rotate(-90);
travel(500);
/*
* travel(500); rotate(90); travel(200); rotate(-90); travel(500);
* rotate(90); travel(200); rotate(90); travel(700); rotate(90);
* travel(200);
*/
while (!Button.ENTER.isDown())
Thread.yield();
LCD.clear();
LCD.drawString("Closing", 0, 0);
if (PC.close())
LCD.drawString("Closed", 0, 0);
try {
Thread.sleep(2000);
} catch (Exception e) {
}
}
public static void main(String[] args) {
PilotAvoid pilot = new PilotAvoid(false);
LCD.clear();
LCD.drawString("PilotRouteMM", 0, 0);
// route.go();
Sound.beep();
while (!Button.ENTER.isDown())
Thread.yield();
Sound.twoBeeps();
Behavior b1 = new FollowRoute(pilot);
Behavior b2 = new AvoidObstacle(pilot);
Behavior[] behaviorList = { b1, b2};
Arbitrator arbitrator = new Arbitrator(behaviorList);
arbitrator.start();
}
}
class FollowRoute implements Behavior {
private static final int TRAVEL = 0;
private static final int ROTATE = 1;
private boolean _suppressed = false;
private PilotAvoid pilot;
private int stepsDone = 0;
public FollowRoute(PilotAvoid 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;
if (stepsDone < 1 && !_suppressed) {
doStep(TRAVEL, 500);
}
if (stepsDone < 2 && !_suppressed) {
doStep(ROTATE, 90);
}
if (stepsDone < 3 && !_suppressed) {
doStep(TRAVEL, 200);
}
if (stepsDone < 4 && !_suppressed) {
doStep(ROTATE, -90);
}
if (stepsDone < 5 && !_suppressed) {
doStep(TRAVEL, 500);
}
Motor.A.stop(true); // not strictly necessary, but good programming practice
Motor.C.stop(true);
}
public void doStep(int moveType, double value) {
switch (moveType) {
case TRAVEL : pilot.travel(value); break;
case ROTATE : pilot.rotate(value); break;
default : System.err.println("Invalid moveType in doStep(): " + moveType); return;
}
stepsDone++;
while (!_suppressed && Motor.A.isMoving()) {
Thread.yield(); // don't exit till suppressed
}
pilot.sendMove(pilot.pilot.getMovement());
}
}
class AvoidObstacle extends Thread implements Behavior {
UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);
private float distance = 255;
private boolean _suppressed = false;
private PilotAvoid pilot;
public AvoidObstacle(PilotAvoid pilot) {
this.pilot = pilot;
this.isDaemon();
this.start();
}
public void run() {
while (true) {
distance = sonar.getDistance();
}
}
public int takeControl() {
return (distance < 15) ? 20 : 0;
}
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
public void action() {
_suppressed = false;
Sound.buzz();
// Take evasive action.
while (!_suppressed) {
Thread.yield(); // don't exit till suppressed
}
// Motor.A.stop(true); // not strictly necessary, but good programming practice
// Motor.C.stop(true);
}
}
......@@ -6,12 +6,13 @@ public class PilotRoute
{
private SlaveIOStreams PC;
private double wheelDiameter = 5.5, trackWidth = 16.0;
private double travelSpeed = 5, rotateSpeed = 45;
private NXTRegulatedMotor left = Motor.B;
private NXTRegulatedMotor right = Motor.C;
double mult = 1.007;
double leftWheelDiameter = 5.5 * mult, rightWheelDiameter = 5.484 * mult, trackWidth = 16.5;
double travelSpeed = 5, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
NXTRegulatedMotor right = Motor.C;
private DifferentialPilot pilot = new DifferentialPilot(wheelDiameter, trackWidth, left, right);
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
public PilotRoute(boolean usb)
{
......
......@@ -2,18 +2,19 @@ import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move;
public class PilotRoute
public class PilotRouteMM
{
private SlaveIOStreams PC;
private double wheelDiameter = 5.5, trackWidth = 16.0;
private double travelSpeed = 5, rotateSpeed = 45;
private NXTRegulatedMotor left = Motor.B;
private NXTRegulatedMotor right = Motor.C;
double mult = 1.007;
double leftWheelDiameter = 55 * mult, rightWheelDiameter = 54.84 * mult, trackWidth = 165;
double travelSpeed = 50, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
NXTRegulatedMotor right = Motor.C;
private DifferentialPilot pilot = new DifferentialPilot(wheelDiameter, trackWidth, left, right);
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
public PilotRoute(boolean usb)
public PilotRouteMM(boolean usb)
{
PC = new SlaveIOStreams(usb);
PC.open();
......@@ -47,11 +48,25 @@ public class PilotRoute
while ( ! Button.ENTER.isDown()) Thread.yield();
Sound.twoBeeps();
travel(50);
travel(500);
rotate(90);
travel(200);
rotate(-90);
travel(500);
/*
travel(500);
rotate(90);
travel(20);
travel(200);
rotate(-90);
travel(50);
travel(500);
rotate(90);
travel(200);
rotate(90);
travel(700);
rotate(90);
travel(200);
*/
while ( ! Button.ENTER.isDown()) Thread.yield();
......@@ -64,10 +79,10 @@ public class PilotRoute
public static void main(String[] args)
{
PilotRoute route = new PilotRoute(false);
PilotRouteMM route = new PilotRouteMM(false);
LCD.clear();
LCD.drawString("Pilot route", 0, 0);
LCD.drawString("PilotRouteMM", 0, 0);
route.go();
}
}
......@@ -30,7 +30,7 @@ public class PilotStraight
public static void main(String [] args)
throws Exception
{
double wheelDiameter = 5.5, trackWidth = 16.0;
double wheelDiameter = 5.5, trackWidth = 16.5;
double travelSpeed = 5, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
NXTRegulatedMotor right = Motor.C;
......
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;
/**
* A program that uses the DifferentialPilot to make a differential
* driven car drive in a square. During the drive the OdometryPoseProvider
* is used to display the Pose of the car on the LCD after each
* movement of the car.
*
* Use the PC tool nxjconsoleviewer to show the LCD output on a PC
* for easier inspection.
*
* @author Ole Caprani
* @version 13.05.15
*/
public class PilotStraight2
{
private static 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)
throws Exception
{
double mult = 1.007;
double leftWheelDiameter = 5.5 * mult, rightWheelDiameter = 5.484 * mult, trackWidth = 16.5;
double travelSpeed = 5, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
NXTRegulatedMotor right = Motor.C;
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
Pose initialPose = new Pose(0,0,0);
pilot.setTravelSpeed(travelSpeed);
pilot.setRotateSpeed(rotateSpeed);
LCD.drawString("PilotStraightLoop", 0, 0);
Button.waitForAnyPress();
while (!Button.ESCAPE.isDown()) {
poseProvider.setPose(initialPose);
pilot.travel(50);
show(poseProvider.getPose());
Button.waitForAnyPress();
}
pilot.stop();
LCD.drawString("Program stopped", 0, 0);
Button.waitForAnyPress();
Thread.sleep(2000);
}
}
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;
/**
* A program that uses the DifferentialPilot to make a differential
* driven car drive in a square. During the drive the OdometryPoseProvider
* is used to display the Pose of the car on the LCD after each
* movement of the car.
*
* Use the PC tool nxjconsoleviewer to show the LCD output on a PC
* for easier inspection.
*
* @author Ole Caprani
* @version 13.05.15
*/
public class PilotTurn2
{
private static 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)
throws Exception
{
double mult = 1.007;
double leftWheelDiameter = 5.5 * mult, rightWheelDiameter = 5.484 * mult, trackWidth = 16.5;
double travelSpeed = 5, rotateSpeed = 45;
NXTRegulatedMotor left = Motor.A;
NXTRegulatedMotor right = Motor.C;
DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, left, right, false);
OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
Pose initialPose = new Pose(0,0,0);
pilot.setTravelSpeed(travelSpeed);
pilot.setRotateSpeed(rotateSpeed);
LCD.drawString("PilotTurn2", 0, 0);
Button.waitForAnyPress();
while (!Button.ESCAPE.isDown()) {
poseProvider.setPose(initialPose);
pilot.rotate(720);
show(poseProvider.getPose());
Button.waitForAnyPress();
}
pilot.stop();
LCD.drawString("Program stopped", 0, 0);
Button.waitForAnyPress();
Thread.sleep(2000);
}
}
......@@ -57,12 +57,11 @@ public class Particle
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()))));
(float) (pose.getX() + xm + (distanceNoiseFactor * move.getDistanceTraveled() * rand.nextGaussian())),
(float) (pose.getY() + ym + (distanceNoiseFactor * move.getDistanceTraveled() * rand.nextGaussian()))));
pose.setHeading(
(float) (pose.getHeading() + move.getAngleTurned() + (angleNoiseFactor * rand.nextGaussian())));
(float) (pose.getHeading() + move.getAngleTurned() + (angleNoiseFactor * move.getAngleTurned() * rand.nextGaussian())));
pose.setHeading((float) ((int) (pose.getHeading() + 0.5f) % 360));
}
}
}
import lejos.geom.*;
import lejos.robotics.*;
import lejos.robotics.mapping.RangeMap;
......@@ -9,89 +8,97 @@ import java.util.Random;
/**
* Represents a particle set for the particle filtering algorithm.